hpp-core  4.13.0
Implement basic classes for canonical path planning for kinematic chains.
problem-solver.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 CNRS
3 // Authors: Florent Lamiraux
4 //
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CORE_PROBLEM_SOLVER_HH
31 #define HPP_CORE_PROBLEM_SOLVER_HH
32 
33 #include <functional>
34 #include <hpp/core/config.hh>
35 #include <hpp/core/container.hh>
36 #include <hpp/core/deprecated.hh>
37 #include <hpp/core/fwd.hh>
38 #include <hpp/pinocchio/fwd.hh>
39 #include <stdexcept>
40 
41 namespace hpp {
42 namespace core {
46 class
48 };
49 typedef std::function<DevicePtr_t(const std::string&)> RobotBuilder_t;
50 typedef std::function<PathOptimizerPtr_t(const ProblemConstPtr_t&)>
52 typedef std::function<PathPlannerPtr_t(const ProblemConstPtr_t&,
53  const RoadmapPtr_t&)>
55 typedef std::function<PathValidationPtr_t(const DevicePtr_t&,
56  const value_type&)>
58 typedef std::function<ConfigValidationPtr_t(const DevicePtr_t&)>
60 typedef std::function<PathProjectorPtr_t(const ProblemConstPtr_t&,
61  const value_type&)>
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> >
72 
79  public:
80  typedef std::vector<PathOptimizerPtr_t> PathOptimizers_t;
81  typedef std::vector<std::string> PathOptimizerTypes_t;
82  typedef std::vector<std::string> ConfigValidationTypes_t;
83 
85  static ProblemSolverPtr_t create();
86 
88  virtual ~ProblemSolver();
89 
92  void robotType(const std::string& type);
93 
95  const std::string& robotType() const;
96 
102  DevicePtr_t createRobot(const std::string& name);
103 
105  virtual void robot(const DevicePtr_t& robot);
106 
108  const DevicePtr_t& robot() const;
109 
111  ProblemPtr_t problem() { return problem_; }
113  const ConfigurationPtr_t& initConfig() const { return initConf_; }
115  virtual void initConfig(const ConfigurationPtr_t& config);
117  const Configurations_t& goalConfigs() const;
119  virtual void addGoalConfig(const ConfigurationPtr_t& config);
121  void resetGoalConfigs();
123  void setGoalConstraints(const NumericalConstraints_t& constraints);
125  void resetGoalConstraints();
126  /*
128  void addGoalConstraint (const ConstraintPtr_t& constraint);
130  void addGoalConstraint (const LockedJointPtr_t& lj);
132  void addGoalConstraint (const std::string& constraintName,
133  const std::string& functionName, const std::size_t priority);
135  void resetGoalConstraint ();
136  */
138  virtual void pathPlannerType(const std::string& type);
139  const std::string& pathPlannerType() const { return pathPlannerType_; }
141  void distanceType(const std::string& type);
142  const std::string& distanceType() const { return distanceType_; }
144  void steeringMethodType(const std::string& type);
145  const std::string& steeringMethodType() const { return steeringMethodType_; }
147  void configurationShooterType(const std::string& type);
148  const std::string& configurationShooterType() const {
149  return configurationShooterType_;
150  }
152  const PathPlannerPtr_t& pathPlanner() const { return pathPlanner_; }
153 
157  void addPathOptimizer(const std::string& type);
159  return pathOptimizerTypes_;
160  }
162  void clearPathOptimizers();
164  const PathOptimizerPtr_t& pathOptimizer(std::size_t rank) const {
165  return pathOptimizers_[rank];
166  }
167 
173  void optimizePath(PathVectorPtr_t path);
174 
180  virtual void pathValidationType(const std::string& type,
181  const value_type& tolerance);
182  const std::string& pathValidationType(value_type& tolerance) const {
183  tolerance = pathValidationTolerance_;
184  return pathValidationType_;
185  }
186 
190  void pathProjectorType(const std::string& type, const value_type& step);
191 
193  const std::string& pathProjectorType(value_type& tolerance) const {
194  tolerance = pathProjectorTolerance_;
195  return pathProjectorType_;
196  }
197 
202  virtual void addConfigValidation(const std::string& type);
203 
206  return configValidationTypes_;
207  }
208 
209  // Clear the vector of config validations
210  void clearConfigValidations();
211 
213  void addConfigValidationBuilder(const std::string& type,
214  const ConfigValidationBuilder_t& builder);
215 
216  const RoadmapPtr_t& roadmap() const { return roadmap_; }
217 
220 
222  void addConstraint(const ConstraintPtr_t& constraint);
223 
225  const ConstraintSetPtr_t& constraints() const { return constraints_; }
226 
228  virtual void resetConstraints();
229 
235  virtual void addNumericalConstraintToConfigProjector(
236  const std::string& configProjName, const std::string& constraintName,
237  const std::size_t priority = 0);
238 
245  void addNumericalConstraint(const std::string& name,
246  const constraints::ImplicitPtr_t& constraint) {
247  numericalConstraints.add(name, constraint);
248  }
249 
252  void comparisonType(const std::string& name, const ComparisonTypes_t types);
253 
254  void comparisonType(const std::string& name, const ComparisonType& type);
255 
256  ComparisonTypes_t comparisonType(const std::string& name) const;
257 
259  constraints::ImplicitPtr_t numericalConstraint(const std::string& name) {
260  return numericalConstraints.get(name, constraints::ImplicitPtr_t());
261  }
262 
273  void computeValueAndJacobian(const Configuration_t& configuration,
274  vector_t& value, matrix_t& jacobian) const;
275 
277  void maxIterProjection(size_type iterations);
279  size_type maxIterProjection() const { return maxIterProjection_; }
280 
282  void maxIterPathPlanning(size_type iterations);
284  size_type maxIterPathPlanning() const { return maxIterPathPlanning_; }
285 
287  void setTimeOutPathPlanning(double timeOut) {
288  timeOutPathPlanning_ = timeOut;
289  }
290 
292  double getTimeOutPathPlanning() { return timeOutPathPlanning_; }
293 
295  void errorThreshold(const value_type& threshold);
297  value_type errorThreshold() const { return errorThreshold_; }
299 
301  virtual void resetProblem();
302 
306  virtual void resetRoadmap();
307 
310 
315  void createPathOptimizers();
316 
320  virtual bool prepareSolveStepByStep();
321 
326  virtual bool executeOneStep();
327 
330  virtual void finishSolveStepByStep();
331 
333  virtual void solve();
334 
346  bool directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate,
347  std::size_t& pathId, std::string& report);
348 
350  void addConfigToRoadmap(const ConfigurationPtr_t& config);
351 
359  void addEdgeToRoadmap(const ConfigurationPtr_t& config1,
360  const ConfigurationPtr_t& config2,
361  const PathPtr_t& path);
362 
364  void interrupt();
365 
367  std::size_t addPath(const PathVectorPtr_t& path) {
368  std::size_t s = paths_.size();
369  paths_.push_back(path);
370  return s;
371  }
372 
374  void erasePath(std::size_t pathId) {
375  PathVectors_t::iterator it = paths_.begin();
376  std::advance(it, pathId);
377 
378  paths_.erase(it);
379  }
380 
382  const PathVectors_t& paths() const { return paths_; }
384 
387 
394  virtual void addObstacle(const DevicePtr_t& device, bool collision,
395  bool distance);
396 
403  virtual void addObstacle(const CollisionObjectPtr_t& inObject, bool collision,
404  bool distance);
405 
408  virtual void removeObstacle(const std::string& name);
409 
416  virtual void addObstacle(const std::string& name,
417  /*const*/ FclCollisionObject& inObject,
418  bool collision, bool distance);
419 
423  void removeObstacleFromJoint(const std::string& jointName,
424  const std::string& obstacleName);
425 
428  void cutObstacle(const std::string& name, const fcl::AABB& aabb);
429 
433  void filterCollisionPairs();
434 
437  CollisionObjectPtr_t obstacle(const std::string& name) const;
438 
441  const Transform3f& obstacleFramePosition(const std::string& name) const;
442 
448  std::list<std::string> obstacleNames(bool collision, bool distance) const;
449 
452  return distanceBetweenObjects_;
453  }
454 
455  pinocchio::GeomModelPtr_t obstacleGeomModel() const { return obstacleModel_; }
456  pinocchio::GeomDataPtr_t obstacleGeomData() const { return obstacleData_; }
458 
460  const ObjectStdVector_t& collisionObstacles() const;
462  const ObjectStdVector_t& distanceObstacles() const;
463 
465  void roadmap(const RoadmapPtr_t& roadmap) { roadmap_ = roadmap; }
466 
470  void initDistance();
471 
475  void initSteeringMethod();
476 
480  void initPathProjector();
481 
483  void initPathValidation();
484 
486  void initConfigValidation();
487 
489  void initValidations();
490 
492  virtual void initProblemTarget();
493 
521 
539 
540  protected:
544  ProblemSolver();
545 
548 
550  void problem(ProblemPtr_t problem);
551 
557  virtual void initializeProblem(ProblemPtr_t problem);
558 
563 
570  std::string pathProjectorType_;
573 
575  std::string pathPlannerType_;
576 
579 
580  private:
582  ConfigurationPtr_t initConf_;
584  Configurations_t goalConfigurations_;
586  std::string robotType_;
588  std::string configurationShooterType_;
590  std::string distanceType_;
592  std::string steeringMethodType_;
594  PathOptimizerTypes_t pathOptimizerTypes_;
595  PathOptimizers_t pathOptimizers_;
597  std::string pathValidationType_;
599  value_type pathValidationTolerance_;
600  // Config validation methods
601  ConfigValidationTypes_t configValidationTypes_;
603  ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
604  ObjectStdVector_t distanceObstacles_; // FIXME should be removed?
605  pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
606  pinocchio::DataPtr_t obstacleRData_; // Contains the frames
607  pinocchio::GeomModelPtr_t obstacleModel_;
608  pinocchio::GeomDataPtr_t obstacleData_;
609  // Tolerance for numerical constraint resolution
610  value_type errorThreshold_;
611  // Maximal number of iterations for numerical constraint resolution
612  size_type maxIterProjection_;
614  unsigned long int maxIterPathPlanning_;
616  double timeOutPathPlanning_;
618  segmentsMap_t passiveDofsMap_;
622  DistanceBetweenObjectsPtr_t distanceBetweenObjects_;
623  void initProblem();
624 }; // class ProblemSolver
625 } // namespace core
626 } // namespace hpp
627 
628 #endif // HPP_CORE_PROBLEM_SOLVER_HH
const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:511
const PathOptimizerPtr_t & pathOptimizer(std::size_t rank) const
Get path optimizer at given rank.
Definition: problem-solver.hh:164
std::size_t addPath(const PathVectorPtr_t &path)
Add a path.
Definition: problem-solver.hh:367
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:123
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
Definition: problem-solver.hh:78
constraints::ComparisonType ComparisonType
Definition: fwd.hh:89
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:182
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:131
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:220
pinocchio::size_type size_type
Definition: fwd.hh:162
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:186
PathPlannerPtr_t pathPlanner_
Definition: problem-solver.hh:564
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:180
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:517
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:514
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:496
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:204
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:505
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:570
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:312
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:456
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:221
const std::string & configurationShooterType() const
Definition: problem-solver.hh:148
Container< JointAndShapes_t > jointAndShapes
Container of JointAndShapes_t.
Definition: problem-solver.hh:534
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:151
std::function< ConfigurationShooterPtr_t(const ProblemConstPtr_t &)> ConfigurationShooterBuilder_t
Definition: problem-solver.hh:64
pinocchio::GeomModelPtr_t obstacleGeomModel() const
Definition: problem-solver.hh:455
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:119
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:566
pinocchio::vector_t vector_t
Definition: fwd.hh:209
Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead lockedJoints
Definition: problem-solver.hh:528
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:538
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:499
pinocchio::value_type value_type
Definition: fwd.hh:163
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:536
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:100
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:560
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:530
pinocchio::Transform3f Transform3f
Definition: fwd.hh:206
const std::string & pathPlannerType() const
Definition: problem-solver.hh:139
shared_ptr< PathOptimizer > PathOptimizerPtr_t
Definition: fwd.hh:179
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:508
double getTimeOutPathPlanning()
set time out for the path planning ( in seconds)
Definition: problem-solver.hh:292
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:188
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:109
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:568
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:223
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:502
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:185
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:572
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:154
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:181
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:575
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:520
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:562
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:465
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:547
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:105
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:578
vector3_t AffordanceConfig_t
Definition: problem-solver.hh:71
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:118
Container< segments_t > passiveDofs
Container of passive DoFs (as segments_t)
Definition: problem-solver.hh:532
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:176
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:296
const DistanceBetweenObjectsPtr_t & distanceBetweenObjects() const
Return list of pair of distance computations.
Definition: problem-solver.hh:451
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:523
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:173
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