30#ifndef HPP_CORE_PROBLEM_SOLVER_HH
31#define HPP_CORE_PROBLEM_SOLVER_HH
38#include <hpp/pinocchio/fwd.hh>
69typedef std::vector<std::pair<std::string, CollisionObjectPtr_t> >
149 return configurationShooterType_;
159 return pathOptimizerTypes_;
165 return pathOptimizers_[rank];
183 tolerance = pathValidationTolerance_;
184 return pathValidationType_;
194 tolerance = pathProjectorTolerance_;
195 return pathProjectorType_;
206 return configValidationTypes_;
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);
260 return numericalConstraints.get(name, constraints::ImplicitPtr_t());
288 timeOutPathPlanning_ = timeOut;
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);
429 bool collision,
bool distance);
435 const std::string& obstacleName);
463 return distanceBetweenObjects_;
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_;
Definition: problem-solver.hh:47
Definition: problem-solver.hh:78
void filterCollisionPairs()
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:522
void clearPathOptimizers()
Clear the vector of path optimizers.
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:80
virtual bool executeOneStep()
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:297
Container< RobotBuilder_t > robots
Definition: problem-solver.hh:507
void problem(ProblemPtr_t problem)
Set pointer to problem.
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:528
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.
virtual void initConfig(ConfigurationIn_t config)
Set initial configuration.
void addNumericalConstraint(const std::string &name, const constraints::ImplicitPtr_t &constraint)
Definition: problem-solver.hh:245
const Configuration_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
std::vector< std::string > ConfigValidationTypes_t
Definition: problem-solver.hh:82
virtual void addObstacle(const CollisionObjectPtr_t &inObject, bool collision, bool distance)
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:164
const DevicePtr_t & robot() const
Get robot.
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:539
void interrupt()
Interrupt path planning and path optimization.
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:558
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:525
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:531
virtual void addConfigValidation(const std::string &type)
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:534
static ProblemSolverPtr_t create()
Create instance and return pointer.
virtual ~ProblemSolver()
Destructor.
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
const RoadmapPtr_t & roadmap() const
Definition: problem-solver.hh:216
virtual void addObstacle(const std::string &name, const CollisionGeometryPtr_t &inObject, const Transform3f &pose, bool collision, bool distance)
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:549
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:543
virtual void resetRoadmap()
std::vector< std::string > PathOptimizerTypes_t
Definition: problem-solver.hh:81
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:510
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:545
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:462
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:373
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:541
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:466
void cutObstacle(const std::string &name, const fcl::AABB &aabb)
Container< SteeringMethodBuilder_t > steeringMethods
Definition: problem-solver.hh:513
void resetGoalConstraints()
Stop defining the goal of path planning as a set of constraints.
void distanceType(const std::string &type)
Set distance type.
void initConfigValidation()
Set config validation by calling config validation factories.
void setGoalConstraints(const NumericalConstraints_t &constraints)
Set goal of path planning as a set of constraints.
virtual void addNumericalConstraintToConfigProjector(const std::string &configProjName, const std::string &constraintName, const std::size_t priority=0)
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:575
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:571
size_type maxIterProjection() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:279
void removeObstacleFromJoint(const std::string &jointName, const std::string &obstacleName)
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:516
virtual void robot(const DevicePtr_t &robot)
Set robot.
virtual void addGoalConfig(ConfigurationIn_t config)
Add goal configuration.
virtual bool prepareSolveStepByStep()
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:467
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
std::list< std::string > obstacleNames(bool collision, bool distance) const
const ObjectStdVector_t & collisionObstacles() const
Local vector of objects considered for collision detection.
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 PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:381
const ConfigValidationTypes_t configValidationTypes()
Get config validation current types.
Definition: problem-solver.hh:205
virtual void finishSolveStepByStep()
void configurationShooterType(const std::string &type)
Set configuration shooter type.
void addConfigToRoadmap(ConfigurationIn_t config)
Add random configuration into roadmap as new node.
void addEdgeToRoadmap(ConfigurationIn_t config1, ConfigurationIn_t config2, const PathPtr_t &path)
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:589
const ObjectStdVector_t & distanceObstacles() const
Local vector of objects considered for distance computation.
void setTimeOutPathPlanning(double timeOut)
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:287
void clearConfigValidations()
void steeringMethodType(const std::string &type)
Set steering method type.
ProblemPtr_t problem_
Problem.
Definition: problem-solver.hh:573
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem-solver.hh:225
const Configurations_t & goalConfigs() const
Get number of goal configuration.
virtual void addObstacle(const std::string &name, FclCollisionObject &inObject, bool collision, bool distance)
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:583
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:577
virtual void addObstacle(const DevicePtr_t &device, bool collision, bool distance)
std::string pathPlannerType_
Path planner.
Definition: problem-solver.hh:586
const std::string & pathProjectorType(value_type &tolerance) const
Get path projector current type and get tolerance.
Definition: problem-solver.hh:193
const std::string & robotType() const
Get robot type.
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:111
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:476
const PathPlannerPtr_t & pathPlanner() const
Get path planner.
Definition: problem-solver.hh:152
void createPathOptimizers()
size_type maxIterPathPlanning() const
Get maximal number of iterations in config projector.
Definition: problem-solver.hh:284
Container< PathValidationBuilder_t > pathValidations
Definition: problem-solver.hh:519
virtual void pathValidationType(const std::string &type, const value_type &tolerance)
const Transform3f & obstacleFramePosition(const std::string &name) const
const PathOptimizerTypes_t & pathOptimizerTypes() const
Definition: problem-solver.hh:158
constraints::ImplicitPtr_t numericalConstraint(const std::string &name)
Get constraint with given name.
Definition: problem-solver.hh:259
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:581
virtual void initializeProblem(ProblemPtr_t problem)
void comparisonType(const std::string &name, const ComparisonType &type)
const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:366
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:547
const std::string & distanceType() const
Definition: problem-solver.hh:142
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:579
void computeValueAndJacobian(const Configuration_t &configuration, vector_t &value, matrix_t &jacobian) const
void pathProjectorType(const std::string &type, const value_type &step)
const std::string & pathValidationType(value_type &tolerance) const
Definition: problem-solver.hh:182
void errorThreshold(const value_type &threshold)
Set error threshold in config projector.
#define HPP_CORE_DLLAPI
Definition: config.hh:88
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:232
pinocchio::value_type value_type
Definition: fwd.hh:174
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:184
std::map< std::string, segments_t > segmentsMap_t
Definition: fwd.hh:231
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:307
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:141
pinocchio::vector3_t vector3_t
Definition: fwd.hh:165
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:215
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:190
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:99
std::vector< std::pair< std::string, CollisionObjectPtr_t > > AffordanceObjects_t
Definition: problem-solver.hh:70
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:129
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:191
std::function< PathValidationPtr_t(const DevicePtr_t &, const value_type &)> PathValidationBuilder_t
Definition: problem-solver.hh:57
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:113
std::function< DevicePtr_t(const std::string &)> RobotBuilder_t
Definition: problem-solver.hh:49
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:323
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:192
std::function< PathOptimizerPtr_t(const ProblemConstPtr_t &)> PathOptimizerBuilder_t
Definition: problem-solver.hh:51
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:199
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:196
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:213
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
std::function< PathPlannerPtr_t(const ProblemConstPtr_t &, const RoadmapPtr_t &)> PathPlannerBuilder_t
Definition: problem-solver.hh:54
pinocchio::vector_t vector_t
Definition: fwd.hh:220
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:108
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:102
pinocchio::size_type size_type
Definition: fwd.hh:173
constraints::ComparisonType ComparisonType
Definition: fwd.hh:90
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:197
pinocchio::Transform3f Transform3f
Definition: fwd.hh:217
std::map< std::string, CenterOfMassComputationPtr_t > CenterOfMassComputationMap_t
Definition: fwd.hh:234
std::function< DistancePtr_t(const ProblemConstPtr_t &)> DistanceBuilder_t
Definition: problem-solver.hh:66
pinocchio::CollisionGeometryPtr_t CollisionGeometryPtr_t
Definition: fwd.hh:101
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:115
shared_ptr< DistanceBetweenObjects > DistanceBetweenObjectsPtr_t
Definition: fwd.hh:142
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:107
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:89
std::function< PathProjectorPtr_t(const ProblemConstPtr_t &, const value_type &)> PathProjectorBuilder_t
Definition: problem-solver.hh:62
std::function< ConfigValidationPtr_t(const DevicePtr_t &)> ConfigValidationBuilder_t
Definition: problem-solver.hh:59
std::function< SteeringMethodPtr_t(const ProblemConstPtr_t &)> SteeringMethodBuilder_t
Definition: problem-solver.hh:68
std::vector< Configuration_t > Configurations_t
Definition: fwd.hh:110
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
Definition: bi-rrt-planner.hh:35
Definition: container.hh:76