hpp::core::ProblemSolver Class Reference

Set and solve a path planning problem. More...

#include <hpp/core/problem-solver.hh>

Public Types

typedef boost::function< PathPlannerPtr_t(const Problem &, const RoadmapPtr_t &) > PathPlannerBuilder_t
 
typedef boost::function< PathOptimizerPtr_t(const Problem &) > PathOptimizerBuilder_t
 
typedef boost::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &) > PathValidationBuilder_t
 
typedef boost::function< PathProjectorPtr_t(const core::DistancePtr_t, value_type) > PathProjectorBuilder_t
 

Public Member Functions

 ProblemSolver ()
 Constructor. More...
 
virtual ~ProblemSolver ()
 Destructor. More...
 
virtual void robot (const DevicePtr_t &robot)
 Set robot. More...
 
const DevicePtr_trobot () const
 Get robot. More...
 
ProblemPtr_t problem ()
 Get pointer to problem. More...
 
const ConfigurationPtr_tinitConfig () const
 Get shared pointer to initial configuration. More...
 
void initConfig (const ConfigurationPtr_t &config)
 Set initial configuration. More...
 
const Configurations_tgoalConfigs () const
 Get number of goal configuration. More...
 
void addGoalConfig (const ConfigurationPtr_t &config)
 Add goal configuration. More...
 
void resetGoalConfigs ()
 Reset the set of goal configurations. More...
 
void pathPlannerType (const std::string &type)
 Set path planner type. More...
 
void addPathPlannerType (const std::string &type, const PathPlannerBuilder_t &builder)
 Add a path planner type. More...
 
const PathPlannerPtr_tpathPlanner () const
 Get path planner. More...
 
void pathOptimizerType (const std::string &type)
 Set path optimizer type. More...
 
const PathOptimizerPtr_tpathOptimizer () const
 Get path optimizer. More...
 
void addPathOptimizerType (const std::string &type, const PathOptimizerBuilder_t &builder)
 Add a path optimizer type. More...
 
void pathValidationType (const std::string &type, const value_type &tolerance)
 Set path validation method. More...
 
void addPathValidationType (const std::string &type, const PathValidationBuilder_t &builder)
 Add a path validation type. More...
 
void pathProjectorType (const std::string &type, const value_type &step)
 Set path projector method. More...
 
void addPathProjectorType (const std::string &type, const PathProjectorBuilder_t &builder)
 Add a path projector type. More...
 
const RoadmapPtr_troadmap () const
 
virtual void resetProblem ()
 Create new problem. More...
 
virtual void resetRoadmap ()
 Reset the roadmap. More...
 
const ObjectVector_tcollisionObstacles () const
 Local vector of objects considered for collision detection. More...
 
const ObjectVector_tdistanceObstacles () const
 Local vector of objects considered for distance computation. More...
 
Constraints
void addConstraint (const ConstraintPtr_t &constraint)
 Add a constraint. More...
 
void addLockedJoint (const LockedJointPtr_t &lockedJoint)
 Add a LockedJoint. More...
 
const ConstraintSetPtr_tconstraints () const
 Get constraint set. More...
 
virtual void resetConstraints ()
 Reset constraint set. More...
 
void addCenterOfMassComputation (const std::string &name, CenterOfMassComputationPtr_t comc)
 Add a CenterOfMassComputation object to the local map. More...
 
CenterOfMassComputationPtr_t centerOfMassComputation (const std::string &name) const
 Get the CenterOfMassComputation object from the local map. More...
 
virtual void addFunctionToConfigProjector (const std::string &constraintName, const std::string &functionName)
 Add differential function to the config projector. More...
 
virtual void addConstraintToConfigProjector (const std::string &constraintName, const DifferentiableFunctionPtr_t &constraint, const ComparisonTypePtr_t comp=Equality::create()) HPP_CORE_DEPRECATED
 Add differentialFunction to the config projector Build the config projector if not constructed. More...
 
void addNumericalConstraint (const std::string &name, const DifferentiableFunctionPtr_t &constraint)
 Add a a numerical constraint in local map. More...
 
void addPassiveDofs (const std::string &name, const SizeIntervals_t &passiveDofs)
 Add a vector of passive dofs in a local map. More...
 
SizeIntervals_t passiveDofs (const std::string &name) const
 Get the vector of passive dofs associated with this name. More...
 
void comparisonType (const std::string &name, const ComparisonType::VectorOfTypes types)
 Set the comparison types of a constraint. More...
 
void comparisonType (const std::string &name, const ComparisonTypePtr_t eq)
 Set the comparison type of a constraint. More...
 
ComparisonTypePtr_t comparisonType (const std::string &name) const
 
DifferentiableFunctionPtr_t numericalConstraint (const std::string &name)
 Get constraint with given name. More...
 
void maxIterations (size_type iterations)
 Set maximal number of iterations in config projector. More...
 
size_type maxIterations () const
 Get maximal number of iterations in config projector. More...
 
void errorThreshold (const value_type &threshold)
 Set error threshold in config projector. More...
 
value_type errorThreshold () const
 Get errorimal number of threshold in config projector. More...
 
Solve problem and get paths
void createPathOptimizer ()
 Create Path optimizer if needed. More...
 
virtual bool prepareSolveStepByStep ()
 Prepare the solver for a step by step planning. More...
 
virtual bool executeOneStep ()
 Execute one step of the planner. More...
 
virtual void finishSolveStepByStep ()
 Finish the solving procedure The path optimizer is not called. More...
 
virtual void solve ()
 Set and solve the problem. More...
 
void addPath (const PathVectorPtr_t &path)
 Add a path. More...
 
const PathVectors_tpaths () const
 Return vector of paths. More...
 
Obstacles
void addObstacle (const CollisionObjectPtr_t &inObject, bool collision, bool distance)
 Add obstacle to the list. More...
 
void removeObstacleFromJoint (const std::string &jointName, const std::string &obstacleName)
 Remove collision pair between a joint and an obstacle. More...
 
const CollisionObjectPtr_tobstacle (const std::string &name)
 Get obstacle by name. More...
 
std::list< std::string > obstacleNames (bool collision, bool distance) const
 Get list of obstacle names. More...
 
const DistanceBetweenObjectsPtr_tdistanceBetweenObjects () const
 Return list of pair of distance computations. More...
 

Protected Member Functions

void problem (ProblemPtr_t problem)
 Set pointer to problem. More...
 
void roadmap (const RoadmapPtr_t &roadmap)
 Set the roadmap. More...
 
void initializeProblem (ProblemPtr_t problem)
 Initialize the new problem. More...
 

Protected Attributes

ConstraintSetPtr_t constraints_
 Store constraints until call to solve. More...
 

Detailed Description

Set and solve a path planning problem.

This class is a container that does the interface between hpp-core library and component to be running in a middleware like CORBA or ROS.

Member Typedef Documentation

◆ PathOptimizerBuilder_t

◆ PathPlannerBuilder_t

◆ PathProjectorBuilder_t

◆ PathValidationBuilder_t

Constructor & Destructor Documentation

◆ ProblemSolver()

hpp::core::ProblemSolver::ProblemSolver ( )

Constructor.

◆ ~ProblemSolver()

virtual hpp::core::ProblemSolver::~ProblemSolver ( )
virtual

Destructor.

Member Function Documentation

◆ addCenterOfMassComputation()

void hpp::core::ProblemSolver::addCenterOfMassComputation ( const std::string &  name,
CenterOfMassComputationPtr_t  comc 
)
inline

Add a CenterOfMassComputation object to the local map.

Parameters
namekey of the object in the map
comcthe object to insert.

◆ addConstraint()

void hpp::core::ProblemSolver::addConstraint ( const ConstraintPtr_t constraint)

Add a constraint.

◆ addConstraintToConfigProjector()

virtual void hpp::core::ProblemSolver::addConstraintToConfigProjector ( const std::string &  constraintName,
const DifferentiableFunctionPtr_t constraint,
const ComparisonTypePtr_t  comp = Equality::create () 
)
inlinevirtual

Add differentialFunction to the config projector Build the config projector if not constructed.

Deprecated:
use addFunctionToConfigProjector instead.

◆ addFunctionToConfigProjector()

virtual void hpp::core::ProblemSolver::addFunctionToConfigProjector ( const std::string &  constraintName,
const std::string &  functionName 
)
virtual

Add differential function to the config projector.

Parameters
constraintNameName given to config projector if created by this method.
functionNamename of the function as stored in internal map. Build the config projector if not yet constructed.

◆ addGoalConfig()

void hpp::core::ProblemSolver::addGoalConfig ( const ConfigurationPtr_t config)

Add goal configuration.

◆ addLockedJoint()

void hpp::core::ProblemSolver::addLockedJoint ( const LockedJointPtr_t lockedJoint)

Add a LockedJoint.

◆ addNumericalConstraint()

void hpp::core::ProblemSolver::addNumericalConstraint ( const std::string &  name,
const DifferentiableFunctionPtr_t constraint 
)
inline

Add a a numerical constraint in local map.

Parameters
namename of the numerical constraint as stored in local map,
constraintnumerical constraint

Numerical constraints are to be inserted in the ConfigProjector of the constraint set.

References hpp::core::ComparisonType::createDefault().

◆ addObstacle()

void hpp::core::ProblemSolver::addObstacle ( const CollisionObjectPtr_t inObject,
bool  collision,
bool  distance 
)

Add obstacle to the list.

Parameters
inObjecta new object.
collisionwhether collision checking should be performed for this object.
distancewhether distance computation should be performed for this object.

◆ addPassiveDofs()

void hpp::core::ProblemSolver::addPassiveDofs ( const std::string &  name,
const SizeIntervals_t passiveDofs 
)
inline

Add a vector of passive dofs in a local map.

Parameters
namethe key of the vector in the map.
passiveDofsa vector of SizeInterval_t interpreted as (index_start, interval_length).

◆ addPath()

void hpp::core::ProblemSolver::addPath ( const PathVectorPtr_t path)
inline

Add a path.

◆ addPathOptimizerType()

void hpp::core::ProblemSolver::addPathOptimizerType ( const std::string &  type,
const PathOptimizerBuilder_t builder 
)
inline

Add a path optimizer type.

Parameters
typename of the new path optimizer type
staticmethod that creates a path optimizer with a problem as input

◆ addPathPlannerType()

void hpp::core::ProblemSolver::addPathPlannerType ( const std::string &  type,
const PathPlannerBuilder_t builder 
)
inline

Add a path planner type.

Parameters
typename of the new path planner type
staticmethod that creates a path planner with a problem and a roadmap as input

◆ addPathProjectorType()

void hpp::core::ProblemSolver::addPathProjectorType ( const std::string &  type,
const PathProjectorBuilder_t builder 
)
inline

Add a path projector type.

Parameters
typename of the new path projector method,
staticmethod that creates a path projector with a distance and tolerance as input.

◆ addPathValidationType()

void hpp::core::ProblemSolver::addPathValidationType ( const std::string &  type,
const PathValidationBuilder_t builder 
)
inline

Add a path validation type.

Parameters
typename of the new path validation method,
staticmethod that creates a path validation with a robot and tolerance as input.

◆ centerOfMassComputation()

CenterOfMassComputationPtr_t hpp::core::ProblemSolver::centerOfMassComputation ( const std::string &  name) const
inline

Get the CenterOfMassComputation object from the local map.

Parameters
namekey of the object in the map
Returns
the corresponding object.
Note
a null shared pointer is returned if the object was not found

◆ collisionObstacles()

const ObjectVector_t& hpp::core::ProblemSolver::collisionObstacles ( ) const

Local vector of objects considered for collision detection.

◆ comparisonType() [1/3]

void hpp::core::ProblemSolver::comparisonType ( const std::string &  name,
const ComparisonType::VectorOfTypes  types 
)
inline

Set the comparison types of a constraint.

Parameters
namename of the differentiable function.

References hpp::core::ComparisonTypes::create().

◆ comparisonType() [2/3]

void hpp::core::ProblemSolver::comparisonType ( const std::string &  name,
const ComparisonTypePtr_t  eq 
)
inline

Set the comparison type of a constraint.

Parameters
namename of the differentiable function.

◆ comparisonType() [3/3]

ComparisonTypePtr_t hpp::core::ProblemSolver::comparisonType ( const std::string &  name) const
inline

◆ constraints()

const ConstraintSetPtr_t& hpp::core::ProblemSolver::constraints ( ) const
inline

Get constraint set.

◆ createPathOptimizer()

void hpp::core::ProblemSolver::createPathOptimizer ( )

Create Path optimizer if needed.

If a path optimizer is already set, do nothing. Type of optimizer is determined by method selectPathOptimizer.

◆ distanceBetweenObjects()

const DistanceBetweenObjectsPtr_t& hpp::core::ProblemSolver::distanceBetweenObjects ( ) const
inline

Return list of pair of distance computations.

◆ distanceObstacles()

const ObjectVector_t& hpp::core::ProblemSolver::distanceObstacles ( ) const

Local vector of objects considered for distance computation.

◆ errorThreshold() [1/2]

void hpp::core::ProblemSolver::errorThreshold ( const value_type threshold)
inline

Set error threshold in config projector.

◆ errorThreshold() [2/2]

value_type hpp::core::ProblemSolver::errorThreshold ( ) const
inline

Get errorimal number of threshold in config projector.

◆ executeOneStep()

virtual bool hpp::core::ProblemSolver::executeOneStep ( )
virtual

Execute one step of the planner.

Returns
the return value of PathPlanner::pathExists of the selected planner.
Note
This won't check if a solution has been found before doing one step. The decision to stop planning is let to the user.

◆ finishSolveStepByStep()

virtual void hpp::core::ProblemSolver::finishSolveStepByStep ( )
virtual

Finish the solving procedure The path optimizer is not called.

◆ goalConfigs()

const Configurations_t& hpp::core::ProblemSolver::goalConfigs ( ) const

Get number of goal configuration.

◆ initConfig() [1/2]

const ConfigurationPtr_t& hpp::core::ProblemSolver::initConfig ( ) const
inline

Get shared pointer to initial configuration.

◆ initConfig() [2/2]

void hpp::core::ProblemSolver::initConfig ( const ConfigurationPtr_t config)

Set initial configuration.

◆ initializeProblem()

void hpp::core::ProblemSolver::initializeProblem ( ProblemPtr_t  problem)
protected

Initialize the new problem.

Parameters
problemis inserted in the ProblemSolver and initialized.
Note
The previous Problem, if any, is not deleted. The function should be called when creating Problem object, in resetProblem() and all reimplementation in inherited class.

◆ maxIterations() [1/2]

void hpp::core::ProblemSolver::maxIterations ( size_type  iterations)
inline

Set maximal number of iterations in config projector.

◆ maxIterations() [2/2]

size_type hpp::core::ProblemSolver::maxIterations ( ) const
inline

Get maximal number of iterations in config projector.

◆ numericalConstraint()

DifferentiableFunctionPtr_t hpp::core::ProblemSolver::numericalConstraint ( const std::string &  name)
inline

Get constraint with given name.

◆ obstacle()

const CollisionObjectPtr_t& hpp::core::ProblemSolver::obstacle ( const std::string &  name)

Get obstacle by name.

◆ obstacleNames()

std::list<std::string> hpp::core::ProblemSolver::obstacleNames ( bool  collision,
bool  distance 
) const

Get list of obstacle names.

Parameters
collisionwhether to return collision obstacle names
distancewhether to return distance obstacle names
Returns
list of obstacle names

◆ passiveDofs()

SizeIntervals_t hpp::core::ProblemSolver::passiveDofs ( const std::string &  name) const
inline

Get the vector of passive dofs associated with this name.

◆ pathOptimizer()

const PathOptimizerPtr_t& hpp::core::ProblemSolver::pathOptimizer ( ) const
inline

Get path optimizer.

◆ pathOptimizerType()

void hpp::core::ProblemSolver::pathOptimizerType ( const std::string &  type)

Set path optimizer type.

◆ pathPlanner()

const PathPlannerPtr_t& hpp::core::ProblemSolver::pathPlanner ( ) const
inline

Get path planner.

◆ pathPlannerType()

void hpp::core::ProblemSolver::pathPlannerType ( const std::string &  type)

Set path planner type.

◆ pathProjectorType()

void hpp::core::ProblemSolver::pathProjectorType ( const std::string &  type,
const value_type step 
)

Set path projector method.

Parameters
typename of new path validation method
stepdiscontinuity tolerance

◆ paths()

const PathVectors_t& hpp::core::ProblemSolver::paths ( ) const
inline

Return vector of paths.

◆ pathValidationType()

void hpp::core::ProblemSolver::pathValidationType ( const std::string &  type,
const value_type tolerance 
)

Set path validation method.

Parameters
typename of new path validation method
toleranceacceptable penetration for path validation Path validation methods are used to validate edges in path planning path optimization methods.

◆ prepareSolveStepByStep()

virtual bool hpp::core::ProblemSolver::prepareSolveStepByStep ( )
virtual

Prepare the solver for a step by step planning.

and try to make direct connections (call PathPlanner::tryDirectPath)

Returns
the return value of PathPlanner::pathExists

◆ problem() [1/2]

ProblemPtr_t hpp::core::ProblemSolver::problem ( )
inline

Get pointer to problem.

◆ problem() [2/2]

void hpp::core::ProblemSolver::problem ( ProblemPtr_t  problem)
inlineprotected

Set pointer to problem.

◆ removeObstacleFromJoint()

void hpp::core::ProblemSolver::removeObstacleFromJoint ( const std::string &  jointName,
const std::string &  obstacleName 
)

Remove collision pair between a joint and an obstacle.

Parameters
jointNamename of the joint,
obstacleNamename of the obstacle

◆ resetConstraints()

virtual void hpp::core::ProblemSolver::resetConstraints ( )
virtual

Reset constraint set.

◆ resetGoalConfigs()

void hpp::core::ProblemSolver::resetGoalConfigs ( )

Reset the set of goal configurations.

◆ resetProblem()

virtual void hpp::core::ProblemSolver::resetProblem ( )
virtual

Create new problem.

◆ resetRoadmap()

virtual void hpp::core::ProblemSolver::resetRoadmap ( )
virtual

Reset the roadmap.

Note
When joints bounds are changed, the roadmap must be reset because the kd tree must be resized.

◆ roadmap() [1/2]

const RoadmapPtr_t& hpp::core::ProblemSolver::roadmap ( ) const
inline

◆ roadmap() [2/2]

void hpp::core::ProblemSolver::roadmap ( const RoadmapPtr_t roadmap)
inlineprotected

Set the roadmap.

◆ robot() [1/2]

virtual void hpp::core::ProblemSolver::robot ( const DevicePtr_t robot)
virtual

Set robot.

◆ robot() [2/2]

const DevicePtr_t& hpp::core::ProblemSolver::robot ( ) const

Get robot.

◆ solve()

virtual void hpp::core::ProblemSolver::solve ( )
virtual

Set and solve the problem.

Member Data Documentation

◆ constraints_

ConstraintSetPtr_t hpp::core::ProblemSolver::constraints_
protected

Store constraints until call to solve.