30 #ifndef HPP_CORE_PROBLEM_SOLVER_HH
31 #define HPP_CORE_PROBLEM_SOLVER_HH
38 #include <hpp/pinocchio/fwd.hh>
69 typedef std::vector<std::pair<std::string, CollisionObjectPtr_t> >
149 return configurationShooterType_;
159 return pathOptimizerTypes_;
165 return pathOptimizers_[rank];
183 tolerance = pathValidationTolerance_;
184 return pathValidationType_;
194 tolerance = pathProjectorTolerance_;
195 return pathProjectorType_;
206 return configValidationTypes_;
236 const std::string& configProjName,
const std::string& constraintName,
237 const std::size_t priority = 0);
246 const constraints::ImplicitPtr_t& constraint) {
247 numericalConstraints.add(name, constraint);
260 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
288 timeOutPathPlanning_ = timeOut;
347 std::size_t& pathId, std::string& report);
368 std::size_t s = paths_.size();
369 paths_.push_back(path);
375 PathVectors_t::iterator it = paths_.begin();
376 std::advance(it, pathId);
418 bool collision,
bool distance);
424 const std::string& obstacleName);
452 return distanceBetweenObjects_;
586 std::string robotType_;
588 std::string configurationShooterType_;
590 std::string distanceType_;
592 std::string steeringMethodType_;
597 std::string pathValidationType_;
605 pinocchio::ModelPtr_t obstacleRModel_;
606 pinocchio::DataPtr_t obstacleRData_;
607 pinocchio::GeomModelPtr_t obstacleModel_;
608 pinocchio::GeomDataPtr_t obstacleData_;
614 unsigned long int maxIterPathPlanning_;
616 double timeOutPathPlanning_;
Definition: problem-solver.hh:47
Definition: problem-solver.hh:78
const ObjectStdVector_t & collisionObstacles() const
Local vector of objects considered for collision detection.
std::list< std::string > obstacleNames(bool collision, bool distance) const
void filterCollisionPairs()
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:511
void clearPathOptimizers()
Clear the vector of path optimizers.
virtual void addGoalConfig(const ConfigurationPtr_t &config)
Add goal configuration.
void resetGoalConfigs()
Reset the set of goal configurations.
void initPathValidation()
Set path validation by calling path validation factory.
void addPathOptimizer(const std::string &type)
void robotType(const std::string &type)
std::vector< PathOptimizerPtr_t > PathOptimizers_t
Definition: problem-solver.hh:80
virtual bool executeOneStep()
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:164
void maxIterPathPlanning(size_type iterations)
Set maximal number of iterations in config projector.
void initSteeringMethod()
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
Definition: problem-solver.hh:297
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:193
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:496
void addConfigToRoadmap(const ConfigurationPtr_t &config)
Add random configuration into roadmap as new node.
void problem(ProblemPtr_t problem)
Set pointer to problem.
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:517
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:216
ComparisonTypes_t comparisonType(const std::string &name) const
virtual void removeObstacle(const std::string &name)
virtual void pathPlannerType(const std::string &type)
Set path planner type.
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition: problem-solver.hh:245
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:82
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
virtual void addObstacle(const CollisionObjectPtr_t &inObject, bool collision, bool distance)
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:528
void interrupt()
Interrupt path planning and path optimization.
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:451
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:547
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:514
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:520
virtual void addConfigValidation(const std::string &type)
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:523
const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
static ProblemSolverPtr_t create()
Create instance and return pointer.
virtual ~ProblemSolver()
Destructor.
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:538
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:532
virtual void resetRoadmap()
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:81
const Configurations_t & goalConfigs() const
Get number of goal configuration.
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:499
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:534
bool directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate, std::size_t &pathId, std::string &report)
CollisionObjectPtr_t obstacle(const std::string &name) const
void erasePath(std::size_t pathId)
Erase a path.
Definition: problem-solver.hh:374
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:530
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:455
void addEdgeToRoadmap(const ConfigurationPtr_t &config1, const ConfigurationPtr_t &config2, const PathPtr_t &path)
const std::string & pathValidationType(value_type &tolerance) const
Definition: problem-solver.hh:182
void cutObstacle(const std::string &name, const fcl::AABB &aabb)
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:502
const std::string & robotType() const
Get robot type.
virtual void initConfig(const ConfigurationPtr_t &config)
Set initial configuration.
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition: problem-solver.hh:158
void resetGoalConstraints()
Stop defining the goal of path planning as a set of constraints.
void distanceType(const std::string &type)
Set distance type.
void initConfigValidation()
Set config validation by calling config validation factories.
const DevicePtr_t & robot() const
Get robot.
void setGoalConstraints(const NumericalConstraints_t &constraints)
Set goal of path planning as a set of constraints.
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
virtual void addNumericalConstraintToConfigProjector(const std::string &configProjName, const std::string &constraintName, const std::size_t priority=0)
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:564
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:560
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:279
void removeObstacleFromJoint(const std::string &jointName, const std::string &obstacleName)
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:505
virtual void robot(const DevicePtr_t &robot)
Set robot.
virtual bool prepareSolveStepByStep()
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:456
virtual void resetConstraints()
Reset constraint set.
void addConstraint(const ConstraintPtr_t &constraint)
Add a constraint.
virtual void initProblemTarget()
Initialize the problem target by calling the path validation factory.
void addConfigValidationBuilder(const std::string &type, const ConfigValidationBuilder_t &builder)
Add a new available config validation method.
void optimizePath(PathVectorPtr_t path)
DevicePtr_t createRobot(const std::string &name)
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition: problem-solver.hh:205
virtual void finishSolveStepByStep()
void configurationShooterType(const std::string &type)
Set configuration shooter type.
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem-solver.hh:225
void comparisonType(const std::string &name, const ComparisonTypes_t types)
void maxIterProjection(size_type iterations)
Set maximal number of iterations in config projector.
ProblemTargetPtr_t target_
Shared pointer to the problem target.
Definition: problem-solver.hh:578
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:152
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:287
void clearConfigValidations()
void steeringMethodType(const std::string &type)
Set steering method type.
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:562
const std::string & distanceType() const
Definition: problem-solver.hh:142
virtual void addObstacle(const std::string &name, FclCollisionObject &inObject, bool collision, bool distance)
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:572
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:566
virtual void addObstacle(const DevicePtr_t &device, bool collision, bool distance)
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:575
const Transform3f & obstacleFramePosition(const std::string &name) const
virtual void resetProblem()
Create new problem.
void initValidations()
Initialize the config and path validations and add the obstacles.
virtual void solve()
Set and solve the problem.
ProblemPtr_t problem()
Get pointer to problem.
Definition: problem-solver.hh:111
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:465
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:382
void createPathOptimizers()
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:284
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:508
virtual void pathValidationType(const std::string &type, const value_type &tolerance)
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition: problem-solver.hh:259
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:570
virtual void initializeProblem(ProblemPtr_t problem)
void comparisonType(const std::string &name, const ComparisonType &type)
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:367
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:536
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:568
void computeValueAndJacobian(const Configuration_t &configuration, vector_t &value, matrix_t &jacobian) const
void pathProjectorType(const std::string &type, const value_type &step)
const ObjectStdVector_t & distanceObstacles() const
Local vector of objects considered for distance computation.
void errorThreshold(const value_type &threshold)
Set error threshold in config projector.
#define HPP_CORE_DLLAPI
Definition: config.hh:64
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:231
pinocchio::value_type value_type
Definition: fwd.hh:173
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:183
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:230
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:192
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:306
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:140
pinocchio::vector3_t vector3_t
Definition: fwd.hh:164
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:214
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:189
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:98
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition: problem-solver.hh:70
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:128
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:190
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &)> PathValidationBuilder_t
Definition: problem-solver.hh:57
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:109
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:112
std::function< DevicePtr_t(const std::string &)> RobotBuilder_t
Definition: problem-solver.hh:49
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:322
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:191
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &)> PathOptimizerBuilder_t
Definition: problem-solver.hh:51
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:198
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:195
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:212
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &)> PathPlannerBuilder_t
Definition: problem-solver.hh:54
pinocchio::vector_t vector_t
Definition: fwd.hh:219
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:100
pinocchio::size_type size_type
Definition: fwd.hh:172
constraints::ComparisonType ComparisonType
Definition: fwd.hh:89
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:196
pinocchio::Transform3f Transform3f
Definition: fwd.hh:216
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:233
std::function< DistancePtr_t(const ProblemConstPtr_t &)> DistanceBuilder_t
Definition: problem-solver.hh:66
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:114
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:141
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:86
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &)> PathProjectorBuilder_t
Definition: problem-solver.hh:62
std::function< ConfigValidationPtr_t(const DevicePtr_t &)> ConfigValidationBuilder_t
Definition: problem-solver.hh:59
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &)> SteeringMethodBuilder_t
Definition: problem-solver.hh:68
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:133
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:129
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:186
pinocchio::matrix_t matrix_t
Definition: fwd.hh:161
Definition: bi-rrt-planner.hh:35