hpp-core  5.0.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 Configuration_t& initConfig() const { return initConf_; }
115  virtual void initConfig(ConfigurationIn_t config);
117  const Configurations_t& goalConfigs() const;
119  virtual void addGoalConfig(ConfigurationIn_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(ConfigurationIn_t config);
351 
359  void addEdgeToRoadmap(ConfigurationIn_t config1, ConfigurationIn_t config2,
360  const PathPtr_t& path);
361 
363  void interrupt();
364 
366  std::size_t addPath(const PathVectorPtr_t& path) {
367  std::size_t s = paths_.size();
368  paths_.push_back(path);
369  return s;
370  }
371 
373  void erasePath(std::size_t pathId) {
374  PathVectors_t::iterator it = paths_.begin();
375  std::advance(it, pathId);
376 
377  paths_.erase(it);
378  }
379 
381  const PathVectors_t& paths() const { return paths_; }
383 
386 
393  virtual void addObstacle(const DevicePtr_t& device, bool collision,
394  bool distance);
395 
402  virtual void addObstacle(const CollisionObjectPtr_t& inObject, bool collision,
403  bool distance);
404 
407  virtual void removeObstacle(const std::string& name);
408 
416  virtual void addObstacle(const std::string& name,
417  const CollisionGeometryPtr_t& inObject,
418  const Transform3f& pose, bool collision,
419  bool distance);
420 
427  virtual void addObstacle(const std::string& name,
428  /*const*/ FclCollisionObject& inObject,
429  bool collision, bool distance);
430 
434  void removeObstacleFromJoint(const std::string& jointName,
435  const std::string& obstacleName);
436 
439  void cutObstacle(const std::string& name, const fcl::AABB& aabb);
440 
444  void filterCollisionPairs();
445 
448  CollisionObjectPtr_t obstacle(const std::string& name) const;
449 
452  const Transform3f& obstacleFramePosition(const std::string& name) const;
453 
459  std::list<std::string> obstacleNames(bool collision, bool distance) const;
460 
463  return distanceBetweenObjects_;
464  }
465 
466  pinocchio::GeomModelPtr_t obstacleGeomModel() const { return obstacleModel_; }
467  pinocchio::GeomDataPtr_t obstacleGeomData() const { return obstacleData_; }
469 
471  const ObjectStdVector_t& collisionObstacles() const;
473  const ObjectStdVector_t& distanceObstacles() const;
474 
476  void roadmap(const RoadmapPtr_t& roadmap) { roadmap_ = roadmap; }
477 
481  void initDistance();
482 
486  void initSteeringMethod();
487 
491  void initPathProjector();
492 
494  void initPathValidation();
495 
497  void initConfigValidation();
498 
500  void initValidations();
501 
503  virtual void initProblemTarget();
504 
532 
550 
551  protected:
555  ProblemSolver();
556 
559 
561  void problem(ProblemPtr_t problem);
562 
568  virtual void initializeProblem(ProblemPtr_t problem);
569 
574 
581  std::string pathProjectorType_;
584 
586  std::string pathPlannerType_;
587 
590 
591  private:
593  Configuration_t initConf_;
595  Configurations_t goalConfigurations_;
597  std::string robotType_;
599  std::string configurationShooterType_;
601  std::string distanceType_;
603  std::string steeringMethodType_;
605  PathOptimizerTypes_t pathOptimizerTypes_;
606  PathOptimizers_t pathOptimizers_;
608  std::string pathValidationType_;
610  value_type pathValidationTolerance_;
611  // Config validation methods
612  ConfigValidationTypes_t configValidationTypes_;
614  ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
615  ObjectStdVector_t distanceObstacles_; // FIXME should be removed?
616  pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
617  pinocchio::DataPtr_t obstacleRData_; // Contains the frames
618  pinocchio::GeomModelPtr_t obstacleModel_;
619  pinocchio::GeomDataPtr_t obstacleData_;
620  // Tolerance for numerical constraint resolution
621  value_type errorThreshold_;
622  // Maximal number of iterations for numerical constraint resolution
623  size_type maxIterProjection_;
625  unsigned long int maxIterPathPlanning_;
627  double timeOutPathPlanning_;
629  segmentsMap_t passiveDofsMap_;
633  DistanceBetweenObjectsPtr_t distanceBetweenObjects_;
634  void initProblem();
635 }; // class ProblemSolver
636 } // namespace core
637 } // namespace hpp
638 
639 #endif // HPP_CORE_PROBLEM_SOLVER_HH
const std::string & steeringMethodType() const
Definition: problem-solver.hh:145
Container< ConfigValidationBuilder_t > configValidations
Definition: problem-solver.hh:522
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:366
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:108
Definition: problem-solver.hh:78
constraints::ComparisonType ComparisonType
Definition: fwd.hh:90
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
Definition: bi-rrt-planner.hh:35
constraints::ComparisonTypes_t ComparisonTypes_t
Definition: fwd.hh:87
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:575
shared_ptr< PathPlanner > PathPlannerPtr_t
Definition: fwd.hh:191
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:99
const std::string & distanceType() const
Definition: problem-solver.hh:142
Container< PathPlannerBuilder_t > pathPlanners
Definition: problem-solver.hh:528
Container< PathProjectorBuilder_t > pathProjectors
Definition: problem-solver.hh:525
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:507
std::vector< PathVectorPtr_t > PathVectors_t
Definition: fwd.hh:215
const Configuration_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem-solver.hh:113
Container< DistanceBuilder_t > distances
Definition: problem-solver.hh:516
std::string pathProjectorType_
Path projector method.
Definition: problem-solver.hh:581
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:323
pinocchio::GeomDataPtr_t obstacleGeomData() const
Definition: problem-solver.hh:467
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:545
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:373
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:466
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
RoadmapPtr_t roadmap_
Store roadmap.
Definition: problem-solver.hh:577
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:539
Container< AffordanceConfig_t > affordanceConfigs
Container of AffordanceConfig_t.
Definition: problem-solver.hh:549
Container< ConfigurationShooterBuilder_t > configurationShooters
Definition: problem-solver.hh:510
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::CollisionGeometryPtr_t CollisionGeometryPtr_t
Definition: fwd.hh:101
Container< AffordanceObjects_t > affordanceObjects
Container of AffordanceObjects_t.
Definition: problem-solver.hh:547
pinocchio::FclCollisionObject FclCollisionObject
Definition: fwd.hh:102
DevicePtr_t robot_
Robot.
Definition: problem-solver.hh:571
Container< CenterOfMassComputationPtr_t > centerOfMassComputations
Container of CenterOfMassComputation.
Definition: problem-solver.hh:541
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:519
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
PathVectors_t paths_
Paths.
Definition: problem-solver.hh:579
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:513
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:196
value_type pathProjectorTolerance_
Tolerance of path projector.
Definition: problem-solver.hh:583
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
std::vector< Configuration_t > Configurations_t
Definition: fwd.hh:110
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:586
Container< PathOptimizerBuilder_t > pathOptimizers
Definition: problem-solver.hh:531
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:573
void roadmap(const RoadmapPtr_t &roadmap)
Set the roadmap.
Definition: problem-solver.hh:476
ConstraintSetPtr_t constraints_
Store constraints until call to solve.
Definition: problem-solver.hh:558
const PathVectors_t & paths() const
Return vector of paths.
Definition: problem-solver.hh:381
#define HPP_CORE_DLLAPI
Definition: config.hh:88
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:107
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:589
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:543
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:462
Container< constraints::ImplicitPtr_t > numericalConstraints
Container of constraints::Implicit.
Definition: problem-solver.hh:534
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