Path Optimization

Namespaces
namespace	hpp::core::pathOptimization

Classes
struct	hpp::core::pathOptimization::ConfigOptimizationTraits
class	hpp::core::pathOptimization::ConfigOptimization
class	hpp::core::pathOptimization::GradientBased
struct	hpp::core::pathOptimization::PartialShortcutTraits
class	hpp::core::pathOptimization::PartialShortcut
struct	hpp::core::pathOptimization::QuadraticProgram
class	hpp::core::pathOptimization::RandomShortcut
class	hpp::core::pathOptimization::SimpleShortcut
class	hpp::core::pathOptimization::SimpleTimeParameterization
class	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >
	Common base for optimization-based path optimizer with splines. More...
struct	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData
class	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >
class	hpp::core::pathOptimization::TrapezoidalTimeParameterization
class	hpp::core::PathOptimizer

Typedefs
typedef std::vector< CollisionConstraintsResult >	hpp::core::pathOptimization::CollisionConstraintsResults_t
typedef Eigen::JacobiSVD< matrix_t >	hpp::core::pathOptimization::QuadraticProgram::Decomposition_t
typedef Eigen::LLT< matrix_t, Eigen::Lower >	hpp::core::pathOptimization::QuadraticProgram::LLT_t
typedef path::Spline< PolynomeBasis, SplineOrder >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Spline
typedef Spline::Ptr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplinePtr_t
typedef std::vector< SplinePtr_t >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Splines_t
typedef SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::Base
typedef shared_ptr< SplineGradientBased >	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::Ptr_t

Enumerations
enum	{ hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::PolynomeBasis = _PolynomeBasis , hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOrder = _SplineOrder }
enum	{ hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::PolynomeBasis = _PolynomeBasis , hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::SplineOrder = _SplineOrder }

Functions
static GradientBasedPtr_t	hpp::core::pathOptimization::GradientBased::create (const ProblemConstPtr_t &problem)
virtual PathVectorPtr_t	hpp::core::pathOptimization::GradientBased::optimize (const PathVectorPtr_t &path)
	Optimize path.
	hpp::core::pathOptimization::GradientBased::GradientBased (const ProblemConstPtr_t &problem)
	hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram (size_type inputSize)
	hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram (const QuadraticProgram &QP, const LinearConstraint &lc)
	hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram (const QuadraticProgram &QP)
	hpp::core::pathOptimization::QuadraticProgram::~QuadraticProgram ()
void	hpp::core::pathOptimization::QuadraticProgram::accuracy (value_type acc)
value_type	hpp::core::pathOptimization::QuadraticProgram::accuracy () const
void	hpp::core::pathOptimization::QuadraticProgram::addRows (const std::size_t &nbRows)
	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineGradientBasedAbstract (const ProblemConstPtr_t &problem)
	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::SplineOptimizationData ()
	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::SplineOptimizationData (size_type rDof)
PathVectorPtr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::buildPathVector (const Splines_t &splines) const
static Ptr_t	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::create (const ProblemConstPtr_t &problem)
virtual PathVectorPtr_t	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::optimize (const PathVectorPtr_t &path)
	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::SplineGradientBased (const ProblemConstPtr_t &problem)
virtual	hpp::core::PathOptimizer::~PathOptimizer ()
ProblemConstPtr_t	hpp::core::PathOptimizer::problem () const
	Get problem.
virtual PathVectorPtr_t	hpp::core::PathOptimizer::optimize (const PathVectorPtr_t &path)=0
	Optimize path.
void	hpp::core::PathOptimizer::interrupt ()
	Interrupt path optimization.
void	hpp::core::PathOptimizer::maxIterations (const unsigned long int &n)
	Set maximal number of iterations.
void	hpp::core::PathOptimizer::timeOut (const double &timeOut)
	set time out (in seconds)
	hpp::core::PathOptimizer::PathOptimizer (const ProblemConstPtr_t &problem)
PathPtr_t	hpp::core::PathOptimizer::steer (ConfigurationIn_t q1, ConfigurationIn_t q2) const
void	hpp::core::PathOptimizer::monitorExecution ()
void	hpp::core::PathOptimizer::endIteration ()
bool	hpp::core::PathOptimizer::shouldStop () const
void	hpp::core::PathOptimizer::initFromParameters ()

Variables
value_type	hpp::core::pathOptimization::QuadraticProgram::accuracy_
ConstraintSetPtr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::set
	The set of constraint of the corresponding path.
shared_ptr< constraints::ExplicitConstraintSet >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::es
	A copy of the explicit solver included in set.
RowBlockIndices	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::activeParameters
DevicePtr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::robot_
value_type	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::alphaInit
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::alwaysStopAtFirst
	If true, consider only one (not all) collision constraint per iteration.
size_type	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::costOrder
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::usePathLengthAsWeights
	If true, the initial path length is used to weight the splines.
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::reorderIntervals
	If true, intervals in collision are checked first at the next iteration.
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::linearizeAtEachStep
	If true, collision constraint will be re-linearized at each iteration.
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::checkJointBound
	If true, joint bounds are enforced.
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::returnOptimum
value_type	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::costThreshold
value_type	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::guessThreshold
value_type	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::QPAccuracy
	Accuracy of QP solver (only used by proxqp).
bool	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::checkOptimum_
bool	hpp::core::PathOptimizer::interrupt_
bool   hpp::core::PathOptimizer::enabled
size_type   hpp::core::PathOptimizer::iteration
boost::posix_time::ptime   hpp::core::PathOptimizer::timeStart

Spline creation
typedef steeringMethod::Spline< PolynomeBasis, SplineOrder >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SSM_t
	Spline steering method.
SSM_t::Ptr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::steeringMethod_
static PathVectorPtr_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::cleanInput (const PathVectorPtr_t &input)
	Flatten path and remove path of zero length.
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline (const StraightPathPtr_t &path, Splines_t &splines) const
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline (const InterpolatedPathPtr_t &path, Splines_t &splines) const
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline (const PathVectorPtr_t &path, Splines_t &splines) const

Path validation
typedef std::vector< std::pair< PathValidationReportPtr_t, std::size_t > >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Reports_t
std::vector< PathValidationPtr_t >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validations_
virtual void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::initializePathValidation (const Splines_t &splines)
Reports_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validatePath (const Splines_t &splines, std::vector< std::size_t > &reordering, bool stopAtFirst, bool reorder) const

Constraint creation
typedef Eigen::RowBlockIndices	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::RowBlockIndices
typedef std::vector< bool >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Bools_t
typedef std::vector< size_type >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Indices_t
typedef std::vector< SplineOptimizationData >	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationDatas_t
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::jointBoundConstraint (const Splines_t &splines, LinearConstraint &lc) const
std::size_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::addBoundConstraints (const Indices_t &bci, const LinearConstraint &bc, Bools_t &activeConstraint, LinearConstraint &constraint) const
	Unused.
Indices_t	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validateBounds (const Splines_t &splines, const LinearConstraint &lc) const
	Mostly for debugging purpose.
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::addContinuityConstraints (const Splines_t &splines, const size_type maxOrder, const SplineOptimizationDatas_t &ess, LinearConstraint &continuity)
	Add the linear constraint to connect consecutive splines together.

Model
matrix_t	hpp::core::pathOptimization::QuadraticProgram::H
vector_t	hpp::core::pathOptimization::QuadraticProgram::b
bool	hpp::core::pathOptimization::QuadraticProgram::bIsZero

Data (for inequality constraints)
LLT_t	hpp::core::pathOptimization::QuadraticProgram::llt
value_type	hpp::core::pathOptimization::QuadraticProgram::trace
Eigen::VectorXi	hpp::core::pathOptimization::QuadraticProgram::activeConstraint
int	hpp::core::pathOptimization::QuadraticProgram::activeSetSize

Data (for equality constraints)
Decomposition_t	hpp::core::pathOptimization::QuadraticProgram::dec
vector_t	hpp::core::pathOptimization::QuadraticProgram::xStar

Program subject to linear equality constraints.
void	hpp::core::pathOptimization::QuadraticProgram::reduced (const LinearConstraint &lc, QuadraticProgram &QPr) const
void	hpp::core::pathOptimization::QuadraticProgram::decompose ()
void	hpp::core::pathOptimization::QuadraticProgram::solve ()

Program subject to linear equality and inequality constraints.
void	hpp::core::pathOptimization::QuadraticProgram::computeLLT ()
double	hpp::core::pathOptimization::QuadraticProgram::solve (const LinearConstraint &ce, const LinearConstraint &ci)

Spline convinience functions
static void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::copy (const Splines_t &in, Splines_t &out)
	Copy a vector of Spline.
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::updateSplines (Splines_t &spline, const vector_t &param) const
void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::updateParameters (vector_t &param, const Splines_t &spline) const
static void	hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::interpolate (const Splines_t &a, const Splines_t &b, const value_type &alpha, Splines_t &res)
	Returns res = (1 - alpha) * a + alpha * b.

Constraint creation
virtual void	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::addProblemConstraints (const PathVectorPtr_t &init, const Splines_t &splines, LinearConstraint &lc, SplineOptimizationDatas_t &sods) const
void	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::addProblemConstraintOnPath (const PathPtr_t &path, const size_type &idxSpline, const SplinePtr_t &spline, LinearConstraint &lc, SplineOptimizationData &sod) const
Eigen::RowBlockIndices	hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::computeActiveParameters (const PathPtr_t &path, const constraints::solver::BySubstitution &hs, const value_type &guessThr=-1, const bool &useExplicitInput=false) const

Detailed Description

Path optimization algorithms derive from class hpp::core::PathOptimizer

Simple shortcut

Find shortest path composed of direct paths between all pairs of waypoints of input path.

To do so, the optimizer builds a roadmap the nodes of which are the input path waypoints and the edges of which are the collision-free output of the steering method between all pairs of nodes.

Typedef Documentation

Base

template<int _PolynomeBasis, int _SplineOrder>

typedef SplineGradientBasedAbstract<_PolynomeBasis, _SplineOrder> hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::Base

Bools_t

template<int _PolynomeBasis, int _SplineOrder>

typedef std::vector<bool> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Bools_t

protected

CollisionConstraintsResults_t

typedef std::vector<CollisionConstraintsResult> hpp::core::pathOptimization::CollisionConstraintsResults_t

Decomposition_t

typedef Eigen::JacobiSVD<matrix_t> hpp::core::pathOptimization::QuadraticProgram::Decomposition_t

Indices_t

template<int _PolynomeBasis, int _SplineOrder>

typedef std::vector<size_type> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Indices_t

protected

LLT_t

typedef Eigen::LLT<matrix_t, Eigen::Lower> hpp::core::pathOptimization::QuadraticProgram::LLT_t

Ptr_t

template<int _PolynomeBasis, int _SplineOrder>

typedef shared_ptr<SplineGradientBased> hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::Ptr_t

Reports_t

template<int _PolynomeBasis, int _SplineOrder>

typedef std::vector<std::pair<PathValidationReportPtr_t, std::size_t> > hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Reports_t

protected

RowBlockIndices

template<int _PolynomeBasis, int _SplineOrder>

typedef Eigen::RowBlockIndices hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::RowBlockIndices

protected

Spline

template<int _PolynomeBasis, int _SplineOrder>

typedef path::Spline<PolynomeBasis, SplineOrder> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Spline

SplineOptimizationDatas_t

template<int _PolynomeBasis, int _SplineOrder>

typedef std::vector<SplineOptimizationData> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationDatas_t

protected

SplinePtr_t

template<int _PolynomeBasis, int _SplineOrder>

typedef Spline::Ptr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplinePtr_t

Splines_t

template<int _PolynomeBasis, int _SplineOrder>

typedef std::vector<SplinePtr_t> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::Splines_t

SSM_t

template<int _PolynomeBasis, int _SplineOrder>

typedef steeringMethod::Spline<PolynomeBasis, SplineOrder> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SSM_t

protected

Spline steering method.

Enumeration Type Documentation

anonymous enum

template<int _PolynomeBasis, int _SplineOrder>

anonymous enum

Enumerator
PolynomeBasis
SplineOrder

anonymous enum

template<int _PolynomeBasis, int _SplineOrder>

anonymous enum

Enumerator
PolynomeBasis
SplineOrder

Function Documentation

accuracy() [1/2]

value_type hpp::core::pathOptimization::QuadraticProgram::accuracy ( ) const

inline

Get accuracy of internal QP solver

Note

only used by proxqp The accuracy corresponds to \(\epsilon_{abs}\) in this paper (Equation (2)).

accuracy() [2/2]

void hpp::core::pathOptimization::QuadraticProgram::accuracy ( value_type acc )

inline

Set accuracy of internal QP solver

Parameters

acc	the desired accuracy

Note

only used by proxqp The accuracy corresponds to \(\epsilon_{abs}\) in this paper (Equation (2)).

addBoundConstraints()

template<int _PolynomeBasis, int _SplineOrder>

std::size_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::addBoundConstraints ( const Indices_t & bci, const LinearConstraint & bc, Bools_t & activeConstraint, LinearConstraint & constraint ) const

protected

Unused.

addContinuityConstraints()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::addContinuityConstraints ( const Splines_t & splines, const size_type maxOrder, const SplineOptimizationDatas_t & ess, LinearConstraint & continuity )

protected

Add the linear constraint to connect consecutive splines together.

addProblemConstraintOnPath()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::addProblemConstraintOnPath ( const PathPtr_t & path, const size_type & idxSpline, const SplinePtr_t & spline, LinearConstraint & lc, SplineOptimizationData & sod ) const

protected

addProblemConstraints()

template<int _PolynomeBasis, int _SplineOrder>

virtual void hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::addProblemConstraints ( const PathVectorPtr_t & init, const Splines_t & splines, LinearConstraint & lc, SplineOptimizationDatas_t & sods ) const

protectedvirtual

Compute a conservative linear representation of the constraints.

It determines:

• which DoFs can be computed explicitely. These DoFs are removed from the variables and computed explicitely.
• which DoFs are constrained implicitely. These DoFs are removed from the variables and are set constant.
• which DoFs are not constrained. These DoFs are kept as variables for optimization.

addRows()

void hpp::core::pathOptimization::QuadraticProgram::addRows ( const std::size_t & nbRows )

inline

appendEquivalentSpline() [1/3]

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline ( const InterpolatedPathPtr_t & path, Splines_t & splines ) const

protected

Convert an interpolated path into a spline and append to vector

Parameters

path	interpolated path,

Return values

splines

vector of splines

Build a spline starting from interpolated path initial configuration and ending at interpolated path final configuration. derivatives (up to an order depending on the spline degree) at start and end are set to zero. If the interpolated path as only two waypoint, treat it as a straight path. Otherwise, throw an exception.

appendEquivalentSpline() [2/3]

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline ( const PathVectorPtr_t & path, Splines_t & splines ) const

protected

For each subpath of path, cast it into a known path and calls appropriate appendEquivalentSpline.

Parameters

splines

the output will be pushed back into this vector.

appendEquivalentSpline() [3/3]

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::appendEquivalentSpline ( const StraightPathPtr_t & path, Splines_t & splines ) const

protected

Convert a straight path into a spline and append to vector

Parameters

path	straight path,

Return values

splines

vector of splines

Build a spline starting from straight path initial configuration and ending at straight path final configuration. derivatives (up to an order depending on the spline degree) at start and end are set to zero.

buildPathVector()

template<int _PolynomeBasis, int _SplineOrder>

PathVectorPtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::buildPathVector ( const Splines_t & splines ) const

protected

cleanInput()

template<int _PolynomeBasis, int _SplineOrder>

PathVectorPtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::cleanInput ( const PathVectorPtr_t & input )

staticprotected

Flatten path and remove path of zero length.

computeActiveParameters()

template<int _PolynomeBasis, int _SplineOrder>

Eigen::RowBlockIndices hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::computeActiveParameters ( const PathPtr_t & path, const constraints::solver::BySubstitution & hs, const value_type & guessThr = -1, const bool & useExplicitInput = false ) const

protected

Parameters

guessThr

Threshold used to check whether the Jacobian contains rows of zeros, in which case the corresponding DoF is considered passive.

computeLLT()

void hpp::core::pathOptimization::QuadraticProgram::computeLLT

(

)

copy()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::copy ( const Splines_t & in, Splines_t & out )

static

Copy a vector of Spline.

create() [1/2]

GradientBasedPtr_t hpp::core::pathOptimization::GradientBased::create ( const ProblemConstPtr_t & problem )

static

Return shared pointer to new object. Default cost is path length.

create() [2/2]

template<int _PolynomeBasis, int _SplineOrder>

Ptr_t hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::create ( const ProblemConstPtr_t & problem )

static

Return shared pointer to new object. Default cost is path length.

decompose()

void hpp::core::pathOptimization::QuadraticProgram::decompose

(

)

endIteration()

void hpp::core::PathOptimizer::endIteration ( )

inlineprotected

GradientBased()

hpp::core::pathOptimization::GradientBased::GradientBased ( const ProblemConstPtr_t & problem )

protected

initFromParameters()

void hpp::core::PathOptimizer::initFromParameters ( )

protected

initializePathValidation()

template<int _PolynomeBasis, int _SplineOrder>

virtual void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::initializePathValidation ( const Splines_t & splines )

protectedvirtual

Initialize validations_. Store a pointer to the path validation of the problem for each spline.

interpolate()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::interpolate ( const Splines_t & a, const Splines_t & b, const value_type & alpha, Splines_t & res )

static

Returns res = (1 - alpha) * a + alpha * b.

interrupt()

void hpp::core::PathOptimizer::interrupt ( )

inline

Interrupt path optimization.

jointBoundConstraint()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::jointBoundConstraint ( const Splines_t & splines, LinearConstraint & lc ) const

protected

maxIterations()

void hpp::core::PathOptimizer::maxIterations

(

const unsigned long int &

n

)

Set maximal number of iterations.

monitorExecution()

void hpp::core::PathOptimizer::monitorExecution ( )

protected

optimize() [1/3]

virtual PathVectorPtr_t hpp::core::pathOptimization::GradientBased::optimize ( const PathVectorPtr_t & path )

virtual

Optimize path.

Implements hpp::core::PathOptimizer.

optimize() [2/3]

template<int _PolynomeBasis, int _SplineOrder>

virtual PathVectorPtr_t hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::optimize ( const PathVectorPtr_t & path )

virtual

Optimize path

• 1) Transform straight paths into splines
• 2) Add continuity constraints
• 3) Add problem constraints
• 4) Make cost function
• 5) Compute explicit representation of linear constraints.
• 6)
• - 6.1) Compute cost hessian
• - 6.2) Compute optimum
• - 6.3) Check path for collision. If no collision, return solution
• - 6.4) add collision constraint
• - 6.5) re-compute explicit representation of linear constraints.
• 7) - Build result path.

Implements hpp::core::PathOptimizer.

optimize() [3/3]

virtual PathVectorPtr_t hpp::core::PathOptimizer::optimize ( const PathVectorPtr_t & path )

pure virtual

Optimize path.

Implemented in hpp::core::pathOptimization::ConfigOptimization, hpp::core::pathOptimization::GradientBased, hpp::core::pathOptimization::PartialShortcut, hpp::core::pathOptimization::RandomShortcut, hpp::core::pathOptimization::RSTimeParameterization, hpp::core::pathOptimization::SimpleShortcut, hpp::core::pathOptimization::SimpleTimeParameterization, hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >, and hpp::core::pathOptimization::TrapezoidalTimeParameterization.

PathOptimizer()

hpp::core::PathOptimizer::PathOptimizer ( const ProblemConstPtr_t & problem )

protected

problem()

ProblemConstPtr_t hpp::core::PathOptimizer::problem ( ) const

inline

Get problem.

QuadraticProgram() [1/3]

hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram ( const QuadraticProgram & QP )

inline

QuadraticProgram() [2/3]

hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram ( const QuadraticProgram & QP, const LinearConstraint & lc )

inline

Constructor

Parameters

inputSize	dimension of the space on which the quadratic cost is defined,
lc	linear equality constraint,

QuadraticProgram() [3/3]

hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram ( size_type inputSize )

inline

Constructor

Parameters

inputSize

dimension of the space on which the quadratic cost is defined,

reduced()

void hpp::core::pathOptimization::QuadraticProgram::reduced ( const LinearConstraint & lc, QuadraticProgram & QPr ) const

inline

shouldStop()

bool hpp::core::PathOptimizer::shouldStop ( ) const

protected

solve() [1/2]

void hpp::core::pathOptimization::QuadraticProgram::solve ( )

inline

solve() [2/2]

double hpp::core::pathOptimization::QuadraticProgram::solve	(	const LinearConstraint &	ce,
		const LinearConstraint &	ci )

Compute solution using quadprog

Parameters

ce	equality constraints
ci	inequality constraints: \( ci.J * x \ge ci.b \)

Returns

the cost of the solution.

Note

computeLLT must have been called before.

if the problem is ill-conditioned, member xStar is left unchanged.

SplineGradientBased()

template<int _PolynomeBasis, int _SplineOrder>

hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::SplineGradientBased ( const ProblemConstPtr_t & problem )

protected

SplineGradientBasedAbstract()

template<int _PolynomeBasis, int _SplineOrder>

hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineGradientBasedAbstract ( const ProblemConstPtr_t & problem )

protected

SplineOptimizationData() [1/2]

template<int _PolynomeBasis, int _SplineOrder>

hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::SplineOptimizationData ( )

inline

SplineOptimizationData() [2/2]

template<int _PolynomeBasis, int _SplineOrder>

hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::SplineOptimizationData ( size_type rDof )

inline

steer()

PathPtr_t hpp::core::PathOptimizer::steer ( ConfigurationIn_t q1, ConfigurationIn_t q2 ) const

protected

timeOut()

void hpp::core::PathOptimizer::timeOut

(

const double &

timeOut

)

set time out (in seconds)

updateParameters()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::updateParameters	(	vector_t &	param,
		const Splines_t &	spline ) const

Gets the parameters each spline.

Todo

make this function static (currently, it only needs the robot number dof.

updateSplines()

template<int _PolynomeBasis, int _SplineOrder>

void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::updateSplines	(	Splines_t &	spline,
		const vector_t &	param ) const

Sets the parameters each spline.

Todo

make this function static (currently, it only needs the robot number dof.

validateBounds()

template<int _PolynomeBasis, int _SplineOrder>

Indices_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validateBounds ( const Splines_t & splines, const LinearConstraint & lc ) const

protected

Mostly for debugging purpose.

validatePath()

template<int _PolynomeBasis, int _SplineOrder>

Reports_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validatePath ( const Splines_t & splines, std::vector< std::size_t > & reordering, bool stopAtFirst, bool reorder ) const

protected

Calls each validations_ on the corresponding spline.

Parameters

reordering	order in which the path validation is run. It is assumed that reordering is a permutation of [0, splines.size()[.
stopAtFirst	if true, then return only at most one report corresponding to the first encountered collision.
reorder	Put the portion in collision first in order to improve performance of next collision check.

~PathOptimizer()

virtual hpp::core::PathOptimizer::~PathOptimizer ( )

inlinevirtual

~QuadraticProgram()

hpp::core::pathOptimization::QuadraticProgram::~QuadraticProgram

(

)

Variable Documentation

accuracy_

value_type hpp::core::pathOptimization::QuadraticProgram::accuracy_

activeConstraint

Eigen::VectorXi hpp::core::pathOptimization::QuadraticProgram::activeConstraint

activeParameters

template<int _PolynomeBasis, int _SplineOrder>

RowBlockIndices hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::activeParameters

Variable on which we can optimize. Other variables are fully constrained.

activeSetSize

int hpp::core::pathOptimization::QuadraticProgram::activeSetSize

alphaInit

template<int _PolynomeBasis, int _SplineOrder>

value_type hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::alphaInit

In [0,1]. Initial value when interpolating between non-colliding current solution and the optimal colliding trajectory.

alwaysStopAtFirst

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::alwaysStopAtFirst

If true, consider only one (not all) collision constraint per iteration.

b

vector_t hpp::core::pathOptimization::QuadraticProgram::b

bIsZero

bool hpp::core::pathOptimization::QuadraticProgram::bIsZero

checkJointBound

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::checkJointBound

If true, joint bounds are enforced.

checkOptimum_

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::checkOptimum_

protected

costOrder

template<int _PolynomeBasis, int _SplineOrder>

size_type hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::costOrder

Order of the derivative used for the optimized cost function (most likely 1, 2, or 3).

costThreshold

template<int _PolynomeBasis, int _SplineOrder>

value_type hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::costThreshold

Stop optimizing if the cost improves less than this threshold between two iterations.

dec

Decomposition_t hpp::core::pathOptimization::QuadraticProgram::dec

[] [1/2]

bool { ... } ::enabled

enabled [2/2]

bool hpp::core::PathOptimizer::enabled

es

template<int _PolynomeBasis, int _SplineOrder>

shared_ptr<constraints::ExplicitConstraintSet> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::es

A copy of the explicit solver included in set.

guessThreshold

template<int _PolynomeBasis, int _SplineOrder>

value_type hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::guessThreshold

Threshold used to check whether the Jacobian contains rows of zeros, in which case the corresponding DoF is considered passive. Negative disables the check.

H

matrix_t hpp::core::pathOptimization::QuadraticProgram::H

interrupt_

bool hpp::core::PathOptimizer::interrupt_

protected

Whether to interrupt computation Set to false at start of optimize method, set to true by method interrupt.

[] [1/2]

size_type { ... } ::iteration

iteration [2/2]

size_type hpp::core::PathOptimizer::iteration

linearizeAtEachStep

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::linearizeAtEachStep

If true, collision constraint will be re-linearized at each iteration.

llt

LLT_t hpp::core::pathOptimization::QuadraticProgram::llt

QPAccuracy

template<int _PolynomeBasis, int _SplineOrder>

value_type hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::QPAccuracy

Accuracy of QP solver (only used by proxqp).

reorderIntervals

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::reorderIntervals

If true, intervals in collision are checked first at the next iteration.

returnOptimum

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::returnOptimum

(For debugging purpose) If true, returns the optimum regardless of collision.

robot_

template<int _PolynomeBasis, int _SplineOrder>

DevicePtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::robot_

protected

set

template<int _PolynomeBasis, int _SplineOrder>

ConstraintSetPtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::set

The set of constraint of the corresponding path.

steeringMethod_

template<int _PolynomeBasis, int _SplineOrder>

SSM_t::Ptr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::steeringMethod_

protected

[] [1/2]

boost::posix_time::ptime { ... } ::timeStart

timeStart [2/2]

boost::posix_time::ptime hpp::core::PathOptimizer::timeStart

trace

value_type hpp::core::pathOptimization::QuadraticProgram::trace

usePathLengthAsWeights

template<int _PolynomeBasis, int _SplineOrder>

bool hpp::core::pathOptimization::SplineGradientBased< _PolynomeBasis, _SplineOrder >::usePathLengthAsWeights

If true, the initial path length is used to weight the splines.

validations_

template<int _PolynomeBasis, int _SplineOrder>

std::vector<PathValidationPtr_t> hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validations_

protected

Path validation Its size is the number of spline paths.

xStar

vector_t hpp::core::pathOptimization::QuadraticProgram::xStar