30 #ifndef HPP_CORE_PROBLEM_SOLVER_HH 31 #define HPP_CORE_PROBLEM_SOLVER_HH 38 #include <hpp/pinocchio/fwd.hh> 50 typedef std::function<PathOptimizerPtr_t(const ProblemConstPtr_t&)>
58 typedef std::function<ConfigValidationPtr_t(const DevicePtr_t&)>
63 typedef std::function<ConfigurationShooterPtr_t(const ProblemConstPtr_t&)>
65 typedef std::function<DistancePtr_t(const ProblemConstPtr_t&)>
67 typedef std::function<SteeringMethodPtr_t(const ProblemConstPtr_t&)>
69 typedef std::vector<std::pair<std::string, CollisionObjectPtr_t> >
92 void robotType(
const std::string& type);
95 const std::string& robotType()
const;
121 void resetGoalConfigs();
125 void resetGoalConstraints();
138 virtual void pathPlannerType(
const std::string& type);
141 void distanceType(
const std::string& type);
144 void steeringMethodType(
const std::string& type);
147 void configurationShooterType(
const std::string& type);
149 return configurationShooterType_;
157 void addPathOptimizer(
const std::string& type);
159 return pathOptimizerTypes_;
162 void clearPathOptimizers();
165 return pathOptimizers_[rank];
180 virtual void pathValidationType(
const std::string& type,
183 tolerance = pathValidationTolerance_;
184 return pathValidationType_;
190 void pathProjectorType(
const std::string& type,
const value_type& step);
194 tolerance = pathProjectorTolerance_;
195 return pathProjectorType_;
202 virtual void addConfigValidation(
const std::string& type);
206 return configValidationTypes_;
210 void clearConfigValidations();
213 void addConfigValidationBuilder(
const std::string& type,
228 virtual void resetConstraints();
235 virtual void addNumericalConstraintToConfigProjector(
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);
254 void comparisonType(
const std::string& name,
const ComparisonType& type);
260 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
277 void maxIterProjection(
size_type iterations);
282 void maxIterPathPlanning(
size_type iterations);
288 timeOutPathPlanning_ = timeOut;
295 void errorThreshold(
const value_type& threshold);
301 virtual void resetProblem();
306 virtual void resetRoadmap();
315 void createPathOptimizers();
320 virtual bool prepareSolveStepByStep();
326 virtual bool executeOneStep();
330 virtual void finishSolveStepByStep();
333 virtual void solve();
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);
394 virtual void addObstacle(
const DevicePtr_t& device,
bool collision,
408 virtual void removeObstacle(
const std::string& name);
416 virtual void addObstacle(
const std::string& name,
418 bool collision,
bool distance);
423 void removeObstacleFromJoint(
const std::string& jointName,
424 const std::string& obstacleName);
428 void cutObstacle(
const std::string& name,
const fcl::AABB& aabb);
433 void filterCollisionPairs();
441 const Transform3f& obstacleFramePosition(
const std::string& name)
const;
448 std::list<std::string> obstacleNames(
bool collision,
bool distance)
const;
452 return distanceBetweenObjects_;
475 void initSteeringMethod();
480 void initPathProjector();
483 void initPathValidation();
486 void initConfigValidation();
489 void initValidations();
492 virtual void initProblemTarget();
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_;
628 #endif // HPP_CORE_PROBLEM_SOLVER_HH const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:511
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:164
Definition: problem-solver.hh:46
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:367
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:123
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
Definition: problem-solver.hh:78
constraints::ComparisonType ComparisonType
Definition: fwd.hh:89
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:182
Definition: bi-rrt-planner.hh:35
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:86
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:131
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition: problem-solver.hh:158
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &)> PathValidationBuilder_t
Definition: problem-solver.hh:57
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:152
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:220
pinocchio::size_type size_type
Definition: fwd.hh:162
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition: problem-solver.hh:205
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &)> PathOptimizerBuilder_t
Definition: problem-solver.hh:51
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:186
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:564
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:180
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:98
const std::string & distanceType() const
Definition: problem-solver.hh:142
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:517
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:514
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:284
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition: problem-solver.hh:245
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:496
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:204
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:505
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:570
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:312
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:456
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:221
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:534
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:193
void erasePath(std::size_t pathId)
Erase a path.
Definition: problem-solver.hh:374
pinocchio::matrix_t matrix_t
Definition: fwd.hh:151
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:455
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:119
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:566
pinocchio::vector_t vector_t
Definition: fwd.hh:209
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:528
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:538
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:499
pinocchio::value_type value_type
Definition: fwd.hh:163
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:536
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:100
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:560
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:530
pinocchio::Transform3f Transform3f
Definition: fwd.hh:206
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:179
ProblemPtr_t problem()
Get pointer to problem.
Definition: problem-solver.hh:111
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &)> PathProjectorBuilder_t
Definition: problem-solver.hh:62
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:508
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:188
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &)> SteeringMethodBuilder_t
Definition: problem-solver.hh:68
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:81
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:109
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:568
std::function< DistancePtr_t(const ProblemConstPtr_t &)> DistanceBuilder_t
Definition: problem-solver.hh:66
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:223
std::function< ConfigValidationPtr_t(const DevicePtr_t &)> ConfigValidationBuilder_t
Definition: problem-solver.hh:59
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &)> PathPlannerBuilder_t
Definition: problem-solver.hh:54
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:502
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:185
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:572
const std::string & pathValidationType(value_type &tolerance) const
Definition: problem-solver.hh:182
std::function< DevicePtr_t(const std::string &)> RobotBuilder_t
Definition: problem-solver.hh:49
pinocchio::vector3_t vector3_t
Definition: fwd.hh:154
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:181
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:216
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition: problem-solver.hh:259
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:82
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:575
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:520
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
Definition: problem-solver.hh:297
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:562
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:465
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:547
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:382
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:279
std::vector< PathOptimizerPtr_t > PathOptimizers_t
Definition: problem-solver.hh:80
ProblemTargetPtr_t target_
Shared pointer to the problem target.
Definition: problem-solver.hh:578
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:118
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:532
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:176
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:296
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:451
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:523
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:173
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition: problem-solver.hh:70
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem-solver.hh:225
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:287