hpp-core  6.0.0
Implement basic classes for canonical path planning for kinematic chains.
Classes | Typedefs | Enumerations | Enumerator | Functions | Variables
Path Optimization

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::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. More...
 
 hpp::core::pathOptimization::GradientBased::GradientBased (const ProblemConstPtr_t &problem)
 
 hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram (size_type inputSize, bool useProxqp=true)
 
 hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram (const QuadraticProgram &QP, const LinearConstraint &lc, bool useProxqp=true)
 
 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. More...
 
virtual PathVectorPtr_t hpp::core::PathOptimizer::optimize (const PathVectorPtr_t &path)=0
 Optimize path. More...
 
void hpp::core::PathOptimizer::interrupt ()
 Interrupt path optimization. More...
 
void hpp::core::PathOptimizer::maxIterations (const unsigned long int &n)
 Set maximal number of iterations. More...
 
void hpp::core::PathOptimizer::timeOut (const double &timeOut)
 set time out (in seconds) More...
 
 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_
 
bool hpp::core::pathOptimization::QuadraticProgram::useProxqp_
 
ConstraintSetPtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::set
 The set of constraint of the corresponding path. More...
 
shared_ptr< constraints::ExplicitConstraintSet > hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::es
 A copy of the explicit solver included in set. More...
 
RowBlockIndices hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::SplineOptimizationData::activeParameters
 
DevicePtr_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::robot_
 
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. More...
 
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. More...
 
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. More...
 
Indices_t hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::validateBounds (const Splines_t &splines, const LinearConstraint &lc) const
 Mostly for debugging purpose. More...
 
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. More...
 

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

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
accthe 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
pathinterpolated path,
Return values
splinesvector 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
splinesthe 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
pathstraight path,
Return values
splinesvector 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>
static 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
guessThrThreshold 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>
static void hpp::core::pathOptimization::SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder >::copy ( const Splines_t &  in,
Splines_t &  out 
)
static

Copy a vector of Spline.

◆ create() [1/2]

static 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>
static 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>
static 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::SplineGradientBased< _PolynomeBasis, _SplineOrder >, hpp::core::pathOptimization::SimpleTimeParameterization, hpp::core::pathOptimization::SimpleShortcut, hpp::core::pathOptimization::RSTimeParameterization, hpp::core::pathOptimization::RandomShortcut, hpp::core::pathOptimization::PartialShortcut, hpp::core::pathOptimization::GradientBased, and hpp::core::pathOptimization::ConfigOptimization.

◆ 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,
bool  useProxqp = true 
)
inline

Constructor

Parameters
inputSizedimension of the space on which the quadratic cost is defined,
lclinear equality constraint,
useProxqpwhether to use proxqp instead of eiquadprog_2011 as the internal solver. This parameter will soon be removed and proxqp be the internal solver.

◆ QuadraticProgram() [3/3]

hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram ( size_type  inputSize,
bool  useProxqp = true 
)
inline

Constructor

Parameters
inputSizedimension of the space on which the quadratic cost is defined,
useProxqpwhether to use proxqp instead of eiquadprog_2011 as the internal solver. This parameter will soon be removed and proxqp be the internal solver.

◆ 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
ceequality constraints
ciinequality 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
reorderingorder in which the path validation is run. It is assumed that reordering is a permutation of [0, splines.size()[.
stopAtFirstif true, then return only at most one report corresponding to the first encountered collision.
reorderPut 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

◆ b

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

◆ bIsZero

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

◆ checkOptimum_

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

◆ dec

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

◆ enabled [1/2]

bool hpp::core::PathOptimizer::enabled

◆  [2/2]

bool { ... } ::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.

◆ 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

◆ llt

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

◆ 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

◆ timeStart [1/2]

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

◆  [2/2]

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

◆ trace

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

◆ useProxqp_

bool hpp::core::pathOptimization::QuadraticProgram::useProxqp_

◆ 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