19 #ifndef HPP_CORE_PROBLEM_SOLVER_HH
20 # define HPP_CORE_PROBLEM_SOLVER_HH
23 # include <functional>
25 # include <hpp/pinocchio/fwd.hh>
135 return pathPlannerType_;
140 return distanceType_;
145 return steeringMethodType_;
150 return configurationShooterType_;
163 return pathOptimizerTypes_;
170 return pathOptimizers_ [rank];
188 tolerance = pathValidationTolerance_;
189 return pathValidationType_;
200 tolerance = pathProjectorTolerance_;
201 return pathProjectorType_;
212 return configValidationTypes_;
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);
277 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
298 return maxIterProjection_;
306 return maxIterPathPlanning_;
311 timeOutPathPlanning_ = timeOut;
316 return timeOutPathPlanning_;
325 return errorThreshold_;
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);
459 const std::string& obstacleName);
463 void cutObstacle (
const std::string& name,
const fcl::AABB& aabb);
489 return distanceBetweenObjects_;
493 {
return obstacleModel_; }
495 {
return obstacleData_; }
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_;
Definition: problem-solver.hh:38
Definition: problem-solver.hh:70
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:553
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:73
virtual bool executeOneStep()
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:168
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:323
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:199
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:538
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:149
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:559
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:222
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:257
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:315
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:75
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:109
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:569
void interrupt()
Interrupt path planning and path optimization.
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:487
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:588
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:556
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:562
virtual void addConfigValidation(const std::string &type)
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:565
const std::string & steeringMethodType() const
Definition: problem-solver.hh:144
static ProblemSolverPtr_t create()
Create instance and return pointer.
virtual ~ProblemSolver()
Destructor.
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:579
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:573
virtual void resetRoadmap()
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:74
const Configurations_t & goalConfigs() const
Get number of goal configuration.
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:541
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:575
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:404
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:571
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:492
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:187
void cutObstacle(const std::string &name, const fcl::AABB &aabb)
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:544
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:162
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.
const std::string & pathPlannerType() const
Definition: problem-solver.hh:134
virtual void addNumericalConstraintToConfigProjector(const std::string &configProjName, const std::string &constraintName, const std::size_t priority=0)
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:605
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:601
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:296
void removeObstacleFromJoint(const std::string &jointName, const std::string &obstacleName)
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:547
virtual void robot(const DevicePtr_t &robot)
Set robot.
virtual bool prepareSolveStepByStep()
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:494
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:211
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:234
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:619
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:153
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:310
void clearConfigValidations()
void steeringMethodType(const std::string &type)
Set steering method type.
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:603
const std::string & distanceType() const
Definition: problem-solver.hh:139
virtual void addObstacle(const std::string &name, FclCollisionObject &inObject, bool collision, bool distance)
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:613
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:607
virtual void addObstacle(const DevicePtr_t &device, bool collision, bool distance)
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:616
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:104
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:504
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:413
void createPathOptimizers()
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:304
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:550
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:275
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:611
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:396
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:577
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:609
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
std::function< ConfigValidationPtr_t(const DevicePtr_t &) > ConfigValidationBuilder_t
Definition: problem-solver.hh:50
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:62
pinocchio::value_type value_type
Definition: fwd.hh:157
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:167
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:213
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:291
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:122
pinocchio::vector3_t vector3_t
Definition: fwd.hh:148
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
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:173
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:89
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:109
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:174
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:100
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:103
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &) > PathValidationBuilder_t
Definition: problem-solver.hh:48
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:307
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:175
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:182
std::function< DevicePtr_t(const std::string &) > RobotBuilder_t
Definition: problem-solver.hh:40
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:179
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:195
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:91
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &) > PathOptimizerBuilder_t
Definition: problem-solver.hh:42
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &) > SteeringMethodBuilder_t
Definition: problem-solver.hh:60
pinocchio::size_type size_type
Definition: fwd.hh:156
constraints::ComparisonType ComparisonType
Definition: fwd.hh:79
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
pinocchio::Transform3f Transform3f
Definition: fwd.hh:199
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:99
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:216
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:105
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:124
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:76
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &) > PathProjectorBuilder_t
Definition: problem-solver.hh:53
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &) > PathPlannerBuilder_t
Definition: problem-solver.hh:45
std::function< DistancePtr_t(const ProblemConstPtr_t &) > DistanceBuilder_t
Definition: problem-solver.hh:58
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &) > ConfigurationShooterBuilder_t
Definition: problem-solver.hh:56
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
Definition: bi-rrt-planner.hh:24