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);
417 virtual void addObstacle(
const std::string& name,
428 virtual void addObstacle(
const std::string& name,
430 bool collision,
bool distance);
435 void removeObstacleFromJoint(
const std::string& jointName,
436 const std::string& obstacleName);
440 void cutObstacle(
const std::string& name,
const fcl::AABB& aabb);
445 void filterCollisionPairs();
453 const Transform3f& obstacleFramePosition(
const std::string& name)
const;
460 std::list<std::string> obstacleNames(
bool collision,
bool distance)
const;
464 return distanceBetweenObjects_;
487 void initSteeringMethod();
492 void initPathProjector();
495 void initPathValidation();
498 void initConfigValidation();
501 void initValidations();
504 virtual void initProblemTarget();
598 std::string robotType_;
600 std::string configurationShooterType_;
602 std::string distanceType_;
604 std::string steeringMethodType_;
609 std::string pathValidationType_;
617 pinocchio::ModelPtr_t obstacleRModel_;
618 pinocchio::DataPtr_t obstacleRData_;
619 pinocchio::GeomModelPtr_t obstacleModel_;
620 pinocchio::GeomDataPtr_t obstacleData_;
626 unsigned long int maxIterPathPlanning_;
628 double timeOutPathPlanning_;
640 #endif // HPP_CORE_PROBLEM_SOLVER_HH const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:523
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:134
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:107
Definition: problem-solver.hh:78
constraints::ComparisonType ComparisonType
Definition: fwd.hh:89
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
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: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:576
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:191
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:529
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:526
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:508
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:215
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:517
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:582
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:323
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:468
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:546
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:162
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:467
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:578
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:540
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:550
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:511
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::CollisionGeometryPtr_t CollisionGeometryPtr_t
Definition: fwd.hh:100
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:548
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:101
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:572
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:542
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:520
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
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:110
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:109
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:580
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:514
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:196
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:584
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: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:587
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:532
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:574
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:477
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:559
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:106
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:590
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:544
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:463
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:535
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