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. | |
virtual | ~ProblemSolver () |
Destructor. | |
virtual void | robot (const DevicePtr_t &robot) |
Set robot. | |
const DevicePtr_t & | robot () const |
Get robot. | |
ProblemPtr_t | problem () |
Get pointer to problem. | |
const ConfigurationPtr_t & | initConfig () const |
Get shared pointer to initial configuration. | |
void | initConfig (const ConfigurationPtr_t &config) |
Set initial configuration. | |
const Configurations_t & | goalConfigs () const |
Get number of goal configuration. | |
void | addGoalConfig (const ConfigurationPtr_t &config) |
Add goal configuration. | |
void | resetGoalConfigs () |
Reset the set of goal configurations. | |
void | pathPlannerType (const std::string &type) |
Set path planner type. | |
void | addPathPlannerType (const std::string &type, const PathPlannerBuilder_t &builder) |
Add a path planner type. | |
const PathPlannerPtr_t & | pathPlanner () const |
Get path planner. | |
void | pathOptimizerType (const std::string &type) |
Set path optimizer type. | |
const PathOptimizerPtr_t & | pathOptimizer () const |
Get path optimizer. | |
void | addPathOptimizerType (const std::string &type, const PathOptimizerBuilder_t &builder) |
Add a path optimizer type. | |
void | pathValidationType (const std::string &type, const value_type &tolerance) |
Set path validation method. | |
void | addPathValidationType (const std::string &type, const PathValidationBuilder_t &builder) |
Add a path validation type. | |
void | pathProjectorType (const std::string &type, const value_type &step) |
Set path projector method. | |
void | addPathProjectorType (const std::string &type, const PathProjectorBuilder_t &builder) |
Add a path projector type. | |
const RoadmapPtr_t & | roadmap () const |
virtual void | resetProblem () |
Create new problem. | |
virtual void | resetRoadmap () |
Reset the roadmap. | |
const ObjectVector_t & | collisionObstacles () const |
Local vector of objects considered for collision detection. | |
const ObjectVector_t & | distanceObstacles () const |
Local vector of objects considered for distance computation. | |
Constraints | |
void | addConstraint (const ConstraintPtr_t &constraint) |
Add a constraint. | |
void | addLockedJoint (const LockedJointPtr_t &lockedJoint) |
Add a LockedJoint. | |
const ConstraintSetPtr_t & | constraints () const |
Get constraint set. | |
virtual void | resetConstraints () |
Reset constraint set. | |
void | addCenterOfMassComputation (const std::string &name, CenterOfMassComputationPtr_t comc) |
Add a CenterOfMassComputation object to the local map. | |
CenterOfMassComputationPtr_t | centerOfMassComputation (const std::string &name) const |
Get the CenterOfMassComputation object from the local map. | |
virtual void | addFunctionToConfigProjector (const std::string &constraintName, const std::string &functionName) |
Add differential function to the config projector. | |
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. | |
void | addNumericalConstraint (const std::string &name, const DifferentiableFunctionPtr_t &constraint) |
Add a a numerical constraint in local map. | |
void | addPassiveDofs (const std::string &name, const SizeIntervals_t &passiveDofs) |
Add a vector of passive dofs in a local map. | |
SizeIntervals_t | passiveDofs (const std::string &name) const |
Get the vector of passive dofs associated with this name. | |
void | comparisonType (const std::string &name, const ComparisonType::VectorOfTypes types) |
Set the comparison types of a constraint. | |
void | comparisonType (const std::string &name, const ComparisonTypePtr_t eq) |
Set the comparison type of a constraint. | |
ComparisonTypePtr_t | comparisonType (const std::string &name) const |
DifferentiableFunctionPtr_t | numericalConstraint (const std::string &name) |
Get constraint with given name. | |
void | maxIterations (size_type iterations) |
Set maximal number of iterations in config projector. | |
size_type | maxIterations () const |
Get maximal number of iterations in config projector. | |
void | errorThreshold (const value_type &threshold) |
Set error threshold in config projector. | |
value_type | errorThreshold () const |
Get errorimal number of threshold in config projector. | |
Solve problem and get paths | |
void | createPathOptimizer () |
Create Path optimizer if needed. | |
virtual bool | prepareSolveStepByStep () |
Prepare the solver for a step by step planning. | |
virtual bool | executeOneStep () |
Execute one step of the planner. | |
virtual void | finishSolveStepByStep () |
Finish the solving procedure The path optimizer is not called. | |
virtual void | solve () |
Set and solve the problem. | |
void | addPath (const PathVectorPtr_t &path) |
Add a path. | |
const PathVectors_t & | paths () const |
Return vector of paths. | |
Obstacles | |
void | addObstacle (const CollisionObjectPtr_t &inObject, bool collision, bool distance) |
Add obstacle to the list. | |
void | removeObstacleFromJoint (const std::string &jointName, const std::string &obstacleName) |
Remove collision pair between a joint and an obstacle. | |
const CollisionObjectPtr_t & | obstacle (const std::string &name) |
Get obstacle by name. | |
std::list< std::string > | obstacleNames (bool collision, bool distance) const |
Get list of obstacle names. | |
const DistanceBetweenObjectsPtr_t & | distanceBetweenObjects () const |
Return list of pair of distance computations. | |
Protected Member Functions | |
void | problem (ProblemPtr_t problem) |
Set pointer to problem. | |
void | roadmap (const RoadmapPtr_t &roadmap) |
Set the roadmap. | |
void | initializeProblem (ProblemPtr_t problem) |
Initialize the new problem. | |
Protected Attributes | |
ConstraintSetPtr_t | constraints_ |
Store constraints until call to solve. |
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.
typedef boost::function< PathOptimizerPtr_t (const Problem&) > hpp::core::ProblemSolver::PathOptimizerBuilder_t |
typedef boost::function< PathPlannerPtr_t (const Problem&, const RoadmapPtr_t&) > hpp::core::ProblemSolver::PathPlannerBuilder_t |
typedef boost::function<PathProjectorPtr_t (const core::DistancePtr_t, value_type) > hpp::core::ProblemSolver::PathProjectorBuilder_t |
typedef boost::function< PathValidationPtr_t (const DevicePtr_t&, const value_type&) > hpp::core::ProblemSolver::PathValidationBuilder_t |
hpp::core::ProblemSolver::ProblemSolver | ( | ) |
Constructor.
virtual hpp::core::ProblemSolver::~ProblemSolver | ( | ) | [virtual] |
Destructor.
void hpp::core::ProblemSolver::addCenterOfMassComputation | ( | const std::string & | name, |
CenterOfMassComputationPtr_t | comc | ||
) | [inline] |
Add a CenterOfMassComputation object to the local map.
name | key of the object in the map |
comc | the object to insert. |
void hpp::core::ProblemSolver::addConstraint | ( | const ConstraintPtr_t & | constraint | ) |
Add a constraint.
virtual void hpp::core::ProblemSolver::addConstraintToConfigProjector | ( | const std::string & | constraintName, |
const DifferentiableFunctionPtr_t & | constraint, | ||
const ComparisonTypePtr_t | comp = Equality::create () |
||
) | [inline, virtual] |
Add differentialFunction to the config projector Build the config projector if not constructed.
virtual void hpp::core::ProblemSolver::addFunctionToConfigProjector | ( | const std::string & | constraintName, |
const std::string & | functionName | ||
) | [virtual] |
Add differential function to the config projector.
constraintName | Name given to config projector if created by this method. |
functionName | name of the function as stored in internal map. Build the config projector if not yet constructed. |
void hpp::core::ProblemSolver::addGoalConfig | ( | const ConfigurationPtr_t & | config | ) |
Add goal configuration.
void hpp::core::ProblemSolver::addLockedJoint | ( | const LockedJointPtr_t & | lockedJoint | ) |
Add a LockedJoint.
void hpp::core::ProblemSolver::addNumericalConstraint | ( | const std::string & | name, |
const DifferentiableFunctionPtr_t & | constraint | ||
) | [inline] |
Add a a numerical constraint in local map.
name | name of the numerical constraint as stored in local map, |
constraint | numerical constraint |
Numerical constraints are to be inserted in the ConfigProjector of the constraint set.
References hpp::core::ComparisonType::createDefault().
void hpp::core::ProblemSolver::addObstacle | ( | const CollisionObjectPtr_t & | inObject, |
bool | collision, | ||
bool | distance | ||
) |
Add obstacle to the list.
inObject | a new object. |
collision | whether collision checking should be performed for this object. |
distance | whether distance computation should be performed for this object. |
void hpp::core::ProblemSolver::addPassiveDofs | ( | const std::string & | name, |
const SizeIntervals_t & | passiveDofs | ||
) | [inline] |
Add a vector of passive dofs in a local map.
name | the key of the vector in the map. |
passiveDofs | a vector of SizeInterval_t interpreted as (index_start, interval_length). |
void hpp::core::ProblemSolver::addPath | ( | const PathVectorPtr_t & | path | ) | [inline] |
Add a path.
void hpp::core::ProblemSolver::addPathOptimizerType | ( | const std::string & | type, |
const PathOptimizerBuilder_t & | builder | ||
) | [inline] |
Add a path optimizer type.
type | name of the new path optimizer type |
static | method that creates a path optimizer with a problem as input |
void hpp::core::ProblemSolver::addPathPlannerType | ( | const std::string & | type, |
const PathPlannerBuilder_t & | builder | ||
) | [inline] |
Add a path planner type.
type | name of the new path planner type |
static | method that creates a path planner with a problem and a roadmap as input |
void hpp::core::ProblemSolver::addPathProjectorType | ( | const std::string & | type, |
const PathProjectorBuilder_t & | builder | ||
) | [inline] |
Add a path projector type.
type | name of the new path projector method, |
static | method that creates a path projector with a distance and tolerance as input. |
void hpp::core::ProblemSolver::addPathValidationType | ( | const std::string & | type, |
const PathValidationBuilder_t & | builder | ||
) | [inline] |
Add a path validation type.
type | name of the new path validation method, |
static | method that creates a path validation with a robot and tolerance as input. |
CenterOfMassComputationPtr_t hpp::core::ProblemSolver::centerOfMassComputation | ( | const std::string & | name | ) | const [inline] |
Get the CenterOfMassComputation object from the local map.
name | key of the object in the map |
const ObjectVector_t& hpp::core::ProblemSolver::collisionObstacles | ( | ) | const |
Local vector of objects considered for collision detection.
void hpp::core::ProblemSolver::comparisonType | ( | const std::string & | name, |
const ComparisonType::VectorOfTypes | types | ||
) | [inline] |
Set the comparison types of a constraint.
name | name of the differentiable function. |
References hpp::core::ComparisonTypes::create().
void hpp::core::ProblemSolver::comparisonType | ( | const std::string & | name, |
const ComparisonTypePtr_t | eq | ||
) | [inline] |
Set the comparison type of a constraint.
name | name of the differentiable function. |
ComparisonTypePtr_t hpp::core::ProblemSolver::comparisonType | ( | const std::string & | name | ) | const [inline] |
References hpp::core::Equality::create().
const ConstraintSetPtr_t& hpp::core::ProblemSolver::constraints | ( | ) | const [inline] |
Get constraint set.
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.
const DistanceBetweenObjectsPtr_t& hpp::core::ProblemSolver::distanceBetweenObjects | ( | ) | const [inline] |
Return list of pair of distance computations.
const ObjectVector_t& hpp::core::ProblemSolver::distanceObstacles | ( | ) | const |
Local vector of objects considered for distance computation.
void hpp::core::ProblemSolver::errorThreshold | ( | const value_type & | threshold | ) | [inline] |
Set error threshold in config projector.
value_type hpp::core::ProblemSolver::errorThreshold | ( | ) | const [inline] |
Get errorimal number of threshold in config projector.
virtual bool hpp::core::ProblemSolver::executeOneStep | ( | ) | [virtual] |
Execute one step of the planner.
virtual void hpp::core::ProblemSolver::finishSolveStepByStep | ( | ) | [virtual] |
Finish the solving procedure The path optimizer is not called.
const Configurations_t& hpp::core::ProblemSolver::goalConfigs | ( | ) | const |
Get number of goal configuration.
const ConfigurationPtr_t& hpp::core::ProblemSolver::initConfig | ( | ) | const [inline] |
Get shared pointer to initial configuration.
void hpp::core::ProblemSolver::initConfig | ( | const ConfigurationPtr_t & | config | ) |
Set initial configuration.
void hpp::core::ProblemSolver::initializeProblem | ( | ProblemPtr_t | problem | ) | [protected] |
Initialize the new problem.
problem | is inserted in the ProblemSolver and initialized. |
void hpp::core::ProblemSolver::maxIterations | ( | size_type | iterations | ) | [inline] |
Set maximal number of iterations in config projector.
size_type hpp::core::ProblemSolver::maxIterations | ( | ) | const [inline] |
Get maximal number of iterations in config projector.
DifferentiableFunctionPtr_t hpp::core::ProblemSolver::numericalConstraint | ( | const std::string & | name | ) | [inline] |
Get constraint with given name.
const CollisionObjectPtr_t& hpp::core::ProblemSolver::obstacle | ( | const std::string & | name | ) |
Get obstacle by name.
std::list<std::string> hpp::core::ProblemSolver::obstacleNames | ( | bool | collision, |
bool | distance | ||
) | const |
Get list of obstacle names.
collision | whether to return collision obstacle names |
distance | whether to return distance obstacle names |
SizeIntervals_t hpp::core::ProblemSolver::passiveDofs | ( | const std::string & | name | ) | const [inline] |
Get the vector of passive dofs associated with this name.
const PathOptimizerPtr_t& hpp::core::ProblemSolver::pathOptimizer | ( | ) | const [inline] |
Get path optimizer.
void hpp::core::ProblemSolver::pathOptimizerType | ( | const std::string & | type | ) |
Set path optimizer type.
const PathPlannerPtr_t& hpp::core::ProblemSolver::pathPlanner | ( | ) | const [inline] |
Get path planner.
void hpp::core::ProblemSolver::pathPlannerType | ( | const std::string & | type | ) |
Set path planner type.
void hpp::core::ProblemSolver::pathProjectorType | ( | const std::string & | type, |
const value_type & | step | ||
) |
Set path projector method.
type | name of new path validation method |
step | discontinuity tolerance |
const PathVectors_t& hpp::core::ProblemSolver::paths | ( | ) | const [inline] |
Return vector of paths.
void hpp::core::ProblemSolver::pathValidationType | ( | const std::string & | type, |
const value_type & | tolerance | ||
) |
Set path validation method.
type | name of new path validation method |
tolerance | acceptable penetration for path validation Path validation methods are used to validate edges in path planning path optimization methods. |
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)
ProblemPtr_t hpp::core::ProblemSolver::problem | ( | ) | [inline] |
Get pointer to problem.
void hpp::core::ProblemSolver::problem | ( | ProblemPtr_t | problem | ) | [inline, protected] |
Set pointer to problem.
void hpp::core::ProblemSolver::removeObstacleFromJoint | ( | const std::string & | jointName, |
const std::string & | obstacleName | ||
) |
Remove collision pair between a joint and an obstacle.
jointName | name of the joint, |
obstacleName | name of the obstacle |
virtual void hpp::core::ProblemSolver::resetConstraints | ( | ) | [virtual] |
Reset constraint set.
void hpp::core::ProblemSolver::resetGoalConfigs | ( | ) |
Reset the set of goal configurations.
virtual void hpp::core::ProblemSolver::resetProblem | ( | ) | [virtual] |
Create new problem.
virtual void hpp::core::ProblemSolver::resetRoadmap | ( | ) | [virtual] |
Reset the roadmap.
const RoadmapPtr_t& hpp::core::ProblemSolver::roadmap | ( | ) | const [inline] |
void hpp::core::ProblemSolver::roadmap | ( | const RoadmapPtr_t & | roadmap | ) | [inline, protected] |
Set the roadmap.
virtual void hpp::core::ProblemSolver::robot | ( | const DevicePtr_t & | robot | ) | [virtual] |
Set robot.
const DevicePtr_t& hpp::core::ProblemSolver::robot | ( | ) | const |
Get robot.
virtual void hpp::core::ProblemSolver::solve | ( | ) | [virtual] |
Set and solve the problem.
Store constraints until call to solve.