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);
367 std::size_t s = paths_.size();
368 paths_.push_back(path);
374 PathVectors_t::iterator it = paths_.begin();
375 std::advance(it, pathId);
393 virtual void addObstacle(
const DevicePtr_t& device,
bool collision,
407 virtual void removeObstacle(
const std::string& name);
416 virtual void addObstacle(
const std::string& name,
427 virtual void addObstacle(
const std::string& name,
429 bool collision,
bool distance);
434 void removeObstacleFromJoint(
const std::string& jointName,
435 const std::string& obstacleName);
439 void cutObstacle(
const std::string& name,
const fcl::AABB& aabb);
444 void filterCollisionPairs();
452 const Transform3f& obstacleFramePosition(
const std::string& name)
const;
459 std::list<std::string> obstacleNames(
bool collision,
bool distance)
const;
463 return distanceBetweenObjects_;
486 void initSteeringMethod();
491 void initPathProjector();
494 void initPathValidation();
497 void initConfigValidation();
500 void initValidations();
503 virtual void initProblemTarget();
597 std::string robotType_;
599 std::string configurationShooterType_;
601 std::string distanceType_;
603 std::string steeringMethodType_;
608 std::string pathValidationType_;
616 pinocchio::ModelPtr_t obstacleRModel_;
617 pinocchio::DataPtr_t obstacleRData_;
618 pinocchio::GeomModelPtr_t obstacleModel_;
619 pinocchio::GeomDataPtr_t obstacleData_;
625 unsigned long int maxIterPathPlanning_;
627 double timeOutPathPlanning_;
639 #endif // HPP_CORE_PROBLEM_SOLVER_HH const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:522
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:366
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:108
Definition: problem-solver.hh:78
constraints::ComparisonType ComparisonType
Definition: fwd.hh:90
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
Definition: bi-rrt-planner.hh:35
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:87
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:142
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:231
pinocchio::size_type size_type
Definition: fwd.hh:173
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:197
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:575
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:191
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:99
const std::string & distanceType() const
Definition: problem-solver.hh:142
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:528
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:525
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:507
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:215
const Configuration_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:516
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:581
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:323
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:467
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:232
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:545
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:373
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:466
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:577
pinocchio::vector_t vector_t
Definition: fwd.hh:220
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:539
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:549
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:510
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::CollisionGeometryPtr_t CollisionGeometryPtr_t
Definition: fwd.hh:101
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:547
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:102
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:571
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:541
pinocchio::Transform3f Transform3f
Definition: fwd.hh:217
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:190
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:519
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:199
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
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:579
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:234
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:513
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:196
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:583
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
std::vector< Configuration_t > Configurations_t
Definition: fwd.hh:110
pinocchio::vector3_t vector3_t
Definition: fwd.hh:165
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:192
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:586
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:531
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:573
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:476
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:558
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:381
#define HPP_CORE_DLLAPI
Definition: config.hh:88
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:107
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:589
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:129
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:543
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:307
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:462
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:534
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:184
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