19 #ifndef HPP_CORE_PROBLEM_SOLVER_HH 20 # define HPP_CORE_PROBLEM_SOLVER_HH 23 # include <functional> 25 # include <hpp/pinocchio/fwd.hh> 41 typedef std::function < PathOptimizerPtr_t (const ProblemConstPtr_t&) >
49 typedef std::function < ConfigValidationPtr_t (const DevicePtr_t&) >
57 typedef std::function <DistancePtr_t (const ProblemConstPtr_t&) >
59 typedef std::function <SteeringMethodPtr_t (const ProblemConstPtr_t&) >
85 void robotType (
const std::string& type);
88 const std::string& robotType ()
const;
120 void resetGoalConfigs ();
133 virtual void pathPlannerType (
const std::string& type);
135 return pathPlannerType_;
138 void distanceType (
const std::string& type);
140 return distanceType_;
143 void steeringMethodType (
const std::string& type);
145 return steeringMethodType_;
148 void configurationShooterType (
const std::string& type);
150 return configurationShooterType_;
161 void addPathOptimizer (
const std::string& type);
163 return pathOptimizerTypes_;
166 void clearPathOptimizers ();
170 return pathOptimizers_ [rank];
185 virtual void pathValidationType (
const std::string& type,
188 tolerance = pathValidationTolerance_;
189 return pathValidationType_;
195 void pathProjectorType (
const std::string& type,
200 tolerance = pathProjectorTolerance_;
201 return pathProjectorType_;
208 virtual void addConfigValidation (
const std::string& type);
212 return configValidationTypes_;
216 void clearConfigValidations ();
219 void addConfigValidationBuilder (
const std::string& type,
240 virtual void resetConstraints ();
247 virtual void addNumericalConstraintToConfigProjector
248 (
const std::string& configProjName,
const std::string& constraintName,
249 const std::size_t priority = 0);
258 const constraints::ImplicitPtr_t&
261 numericalConstraints.add (name, constraint);
266 void comparisonType (
const std::string& name,
269 void comparisonType (
const std::string& name,
277 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
294 void maxIterProjection (
size_type iterations);
298 return maxIterProjection_;
302 void maxIterPathPlanning (
size_type iterations);
306 return maxIterPathPlanning_;
311 timeOutPathPlanning_ = timeOut;
316 return timeOutPathPlanning_;
321 void errorThreshold (
const value_type& threshold);
325 return errorThreshold_;
330 virtual void resetProblem ();
335 virtual void resetRoadmap ();
344 void createPathOptimizers ();
349 virtual bool prepareSolveStepByStep ();
355 virtual bool executeOneStep ();
359 virtual void finishSolveStepByStep ();
362 virtual void solve ();
376 bool validate, std::size_t& pathId, std::string& report);
398 std::size_t s = paths_.size();
399 paths_.push_back (path);
406 PathVectors_t::iterator it = paths_.begin();
407 std::advance(it, pathId);
428 virtual void addObstacle (
const DevicePtr_t& device,
bool collision,
442 virtual void removeObstacle (
const std::string& name);
450 virtual void addObstacle (
const std::string& name,
458 void removeObstacleFromJoint (
const std::string& jointName,
459 const std::string& obstacleName);
463 void cutObstacle (
const std::string& name,
const fcl::AABB& aabb);
468 void filterCollisionPairs ();
476 const Transform3f& obstacleFramePosition (
const std::string& name)
const;
483 std::list <std::string> obstacleNames (
bool collision,
bool distance)
489 return distanceBetweenObjects_;
493 {
return obstacleModel_; }
495 {
return obstacleData_; }
512 void initDistance ();
517 void initSteeringMethod ();
522 void initPathProjector ();
525 void initPathValidation ();
528 void initConfigValidation ();
531 void initValidations ();
534 virtual void initProblemTarget ();
626 std::string robotType_;
628 std::string configurationShooterType_;
630 std::string distanceType_;
632 std::string steeringMethodType_;
637 std::string pathValidationType_;
645 pinocchio::ModelPtr_t obstacleRModel_;
646 pinocchio::DataPtr_t obstacleRData_;
647 pinocchio::GeomModelPtr_t obstacleModel_;
648 pinocchio::GeomDataPtr_t obstacleData_;
654 unsigned long int maxIterPathPlanning_;
656 double timeOutPathPlanning_;
669 #endif // HPP_CORE_PROBLEM_SOLVER_HH const std::string & steeringMethodType() const
Definition: problem-solver.hh:144
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:553
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:168
Definition: problem-solver.hh:37
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:103
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:396
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
Definition: problem-solver.hh:69
constraints::ComparisonType ComparisonType
Definition: fwd.hh:79
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &) > ConfigurationShooterBuilder_t
Definition: problem-solver.hh:56
Definition: bi-rrt-planner.hh:24
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:109
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:76
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:124
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition: problem-solver.hh:162
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:153
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:213
pinocchio::size_type size_type
Definition: fwd.hh:156
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition: problem-solver.hh:211
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:605
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &) > PathValidationBuilder_t
Definition: problem-solver.hh:48
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:174
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:89
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &) > PathProjectorBuilder_t
Definition: problem-solver.hh:53
const std::string & distanceType() const
Definition: problem-solver.hh:139
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:559
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:556
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:304
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition: problem-solver.hh:257
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:538
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:197
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition: problem-solver.hh:61
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:547
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:611
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:307
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:494
const std::string & configurationShooterType() const
Definition: problem-solver.hh:149
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:575
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:199
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &) > PathOptimizerBuilder_t
Definition: problem-solver.hh:42
void erasePath(std::size_t pathId)
Erase a path.
Definition: problem-solver.hh:404
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
std::function< DistancePtr_t(const ProblemConstPtr_t &) > DistanceBuilder_t
Definition: problem-solver.hh:58
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:492
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:607
pinocchio::vector_t vector_t
Definition: fwd.hh:202
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:569
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:579
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:541
pinocchio::value_type value_type
Definition: fwd.hh:157
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:577
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:91
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:601
std::function< DevicePtr_t(const std::string &) > RobotBuilder_t
Definition: problem-solver.hh:40
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:571
pinocchio::Transform3f Transform3f
Definition: fwd.hh:199
std::function< ConfigValidationPtr_t(const DevicePtr_t &) > ConfigValidationBuilder_t
Definition: problem-solver.hh:50
const std::string & pathPlannerType() const
Definition: problem-solver.hh:134
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:173
ProblemPtr_t problem()
Get pointer to problem.
Definition: problem-solver.hh:104
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:550
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:315
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:182
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:74
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:100
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:99
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:609
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:216
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:544
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:179
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:613
const std::string & pathValidationType(value_type &tolerance) const
Definition: problem-solver.hh:187
pinocchio::vector3_t vector3_t
Definition: fwd.hh:148
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:175
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:222
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition: problem-solver.hh:275
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:75
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:616
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:562
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
Definition: problem-solver.hh:323
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:603
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:504
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:588
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &) > PathPlannerBuilder_t
Definition: problem-solver.hh:45
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:413
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:296
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &) > SteeringMethodBuilder_t
Definition: problem-solver.hh:60
std::vector< PathOptimizerPtr_t > PathOptimizers_t
Definition: problem-solver.hh:73
ProblemTargetPtr_t target_
Shared pointer to the problem target.
Definition: problem-solver.hh:619
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:62
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:109
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:573
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:291
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:487
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:565
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:167
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem-solver.hh:234
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:310