hpp-core  4.12.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 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_PROBLEM_SOLVER_HH
20 # define HPP_CORE_PROBLEM_SOLVER_HH
21 
22 # include <stdexcept>
23 # include <functional>
24 
25 # include <hpp/pinocchio/fwd.hh>
26 
27 # include <hpp/core/fwd.hh>
28 # include <hpp/core/config.hh>
29 # include <hpp/core/deprecated.hh>
30 # include <hpp/core/container.hh>
31 
32 namespace hpp {
33  namespace core {
38  {
39  };
40  typedef std::function < DevicePtr_t (const std::string&) > RobotBuilder_t;
41  typedef std::function < PathOptimizerPtr_t (const ProblemConstPtr_t&) >
43  typedef std::function < PathPlannerPtr_t (const ProblemConstPtr_t&,
44  const RoadmapPtr_t&) >
46  typedef std::function < PathValidationPtr_t (const DevicePtr_t&,
47  const value_type&) >
49  typedef std::function < ConfigValidationPtr_t (const DevicePtr_t&) >
51  typedef std::function <PathProjectorPtr_t (const ProblemConstPtr_t&,
52  const value_type&) >
54  typedef std::function <ConfigurationShooterPtr_t
55  (const ProblemConstPtr_t&) >
57  typedef std::function <DistancePtr_t (const ProblemConstPtr_t&) >
59  typedef std::function <SteeringMethodPtr_t (const ProblemConstPtr_t&) >
61  typedef std::vector<std::pair < std::string, CollisionObjectPtr_t > > AffordanceObjects_t;
63 
70  {
71  public:
72 
73  typedef std::vector <PathOptimizerPtr_t> PathOptimizers_t;
74  typedef std::vector <std::string> PathOptimizerTypes_t;
75  typedef std::vector <std::string> ConfigValidationTypes_t;
76 
78  static ProblemSolverPtr_t create ();
79 
81  virtual ~ProblemSolver ();
82 
85  void robotType (const std::string& type);
86 
88  const std::string& robotType () const;
89 
95  DevicePtr_t createRobot (const std::string& name);
96 
98  virtual void robot (const DevicePtr_t& robot);
99 
101  const DevicePtr_t& robot () const;
102 
105  {
106  return problem_;
107  }
110  {
111  return initConf_;
112  }
114  virtual void initConfig (const ConfigurationPtr_t& config);
116  const Configurations_t& goalConfigs () const;
118  virtual void addGoalConfig (const ConfigurationPtr_t& config);
120  void resetGoalConfigs ();
121  /*
123  void addGoalConstraint (const ConstraintPtr_t& constraint);
125  void addGoalConstraint (const LockedJointPtr_t& lj);
127  void addGoalConstraint (const std::string& constraintName,
128  const std::string& functionName, const std::size_t priority);
130  void resetGoalConstraint ();
131  */
133  virtual void pathPlannerType (const std::string& type);
134  const std::string& pathPlannerType () const {
135  return pathPlannerType_;
136  }
138  void distanceType (const std::string& type);
139  const std::string& distanceType () const {
140  return distanceType_;
141  }
143  void steeringMethodType (const std::string& type);
144  const std::string& steeringMethodType () const {
145  return steeringMethodType_;
146  }
148  void configurationShooterType (const std::string& type);
149  const std::string& configurationShooterType () const {
150  return configurationShooterType_;
151  }
154  {
155  return pathPlanner_;
156  }
157 
161  void addPathOptimizer (const std::string& type);
162  const PathOptimizerTypes_t& pathOptimizerTypes () const {
163  return pathOptimizerTypes_;
164  }
166  void clearPathOptimizers ();
168  const PathOptimizerPtr_t& pathOptimizer (std::size_t rank) const
169  {
170  return pathOptimizers_ [rank];
171  }
172 
178  void optimizePath (PathVectorPtr_t path);
179 
185  virtual void pathValidationType (const std::string& type,
186  const value_type& tolerance);
187  const std::string& pathValidationType (value_type& tolerance) const {
188  tolerance = pathValidationTolerance_;
189  return pathValidationType_;
190  }
191 
195  void pathProjectorType (const std::string& type,
196  const value_type& step);
197 
199  const std::string& pathProjectorType (value_type& tolerance) const {
200  tolerance = pathProjectorTolerance_;
201  return pathProjectorType_;
202  }
203 
208  virtual void addConfigValidation (const std::string& type);
209 
211  const ConfigValidationTypes_t configValidationTypes () {
212  return configValidationTypes_;
213  }
214 
215  // Clear the vector of config validations
216  void clearConfigValidations ();
217 
219  void addConfigValidationBuilder (const std::string& type,
220  const ConfigValidationBuilder_t& builder);
221 
222  const RoadmapPtr_t& roadmap () const
223  {
224  return roadmap_;
225  }
226 
229 
231  void addConstraint (const ConstraintPtr_t& constraint);
232 
235  {
236  return constraints_;
237  }
238 
240  virtual void resetConstraints ();
241 
247  virtual void addNumericalConstraintToConfigProjector
248  (const std::string& configProjName, const std::string& constraintName,
249  const std::size_t priority = 0);
250 
257  void addNumericalConstraint (const std::string& name,
258  const constraints::ImplicitPtr_t&
259  constraint)
260  {
261  numericalConstraints.add (name, constraint);
262  }
263 
266  void comparisonType (const std::string& name,
267  const ComparisonTypes_t types);
268 
269  void comparisonType (const std::string& name,
270  const ComparisonType &type);
271 
272  ComparisonTypes_t comparisonType (const std::string& name) const;
273 
275  constraints::ImplicitPtr_t numericalConstraint (const std::string& name)
276  {
277  return numericalConstraints.get(name, constraints::ImplicitPtr_t());
278  }
279 
290  void computeValueAndJacobian (const Configuration_t& configuration,
291  vector_t& value, matrix_t& jacobian) const;
292 
294  void maxIterProjection (size_type iterations);
297  {
298  return maxIterProjection_;
299  }
300 
302  void maxIterPathPlanning (size_type iterations);
305  {
306  return maxIterPathPlanning_;
307  }
308 
310  void setTimeOutPathPlanning(double timeOut){
311  timeOutPathPlanning_ = timeOut;
312  }
313 
316  return timeOutPathPlanning_;
317  }
318 
319 
321  void errorThreshold (const value_type& threshold);
324  {
325  return errorThreshold_;
326  }
328 
330  virtual void resetProblem ();
331 
335  virtual void resetRoadmap ();
336 
339 
344  void createPathOptimizers ();
345 
349  virtual bool prepareSolveStepByStep ();
350 
355  virtual bool executeOneStep ();
356 
359  virtual void finishSolveStepByStep ();
360 
362  virtual void solve ();
363 
375  bool directPath (ConfigurationIn_t start, ConfigurationIn_t end,
376  bool validate, std::size_t& pathId, std::string& report);
377 
379  void addConfigToRoadmap (const ConfigurationPtr_t& config);
380 
388  void addEdgeToRoadmap (const ConfigurationPtr_t& config1,
389  const ConfigurationPtr_t& config2,
390  const PathPtr_t& path);
391 
393  void interrupt ();
394 
396  std::size_t addPath (const PathVectorPtr_t& path)
397  {
398  std::size_t s = paths_.size();
399  paths_.push_back (path);
400  return s;
401  }
402 
404  void erasePath (std::size_t pathId)
405  {
406  PathVectors_t::iterator it = paths_.begin();
407  std::advance(it, pathId);
408 
409  paths_.erase(it);
410  }
411 
413  const PathVectors_t& paths () const
414  {
415  return paths_;
416  }
418 
421 
428  virtual void addObstacle (const DevicePtr_t& device, bool collision,
429  bool distance);
430 
437  virtual void addObstacle (const CollisionObjectPtr_t &inObject, bool collision,
438  bool distance);
439 
442  virtual void removeObstacle (const std::string& name);
443 
450  virtual void addObstacle (const std::string& name,
451  /*const*/ FclCollisionObject &inObject,
452  bool collision,
453  bool distance);
454 
458  void removeObstacleFromJoint (const std::string& jointName,
459  const std::string& obstacleName);
460 
463  void cutObstacle (const std::string& name, const fcl::AABB& aabb);
464 
468  void filterCollisionPairs ();
469 
472  CollisionObjectPtr_t obstacle (const std::string& name) const;
473 
476  const Transform3f& obstacleFramePosition (const std::string& name) const;
477 
483  std::list <std::string> obstacleNames (bool collision, bool distance)
484  const;
485 
488  {
489  return distanceBetweenObjects_;
490  }
491 
492  pinocchio::GeomModelPtr_t obstacleGeomModel () const
493  { return obstacleModel_; }
494  pinocchio::GeomDataPtr_t obstacleGeomData () const
495  { return obstacleData_; }
497 
499  const ObjectStdVector_t& collisionObstacles () const;
501  const ObjectStdVector_t& distanceObstacles () const;
502 
504  void roadmap (const RoadmapPtr_t& roadmap)
505  {
506  roadmap_ = roadmap;
507  }
508 
512  void initDistance ();
513 
517  void initSteeringMethod ();
518 
522  void initPathProjector ();
523 
525  void initPathValidation ();
526 
528  void initConfigValidation ();
529 
531  void initValidations ();
532 
534  virtual void initProblemTarget ();
535 
563 
580 
581  protected:
585  ProblemSolver ();
586 
589 
591  void problem (ProblemPtr_t problem);
592 
598  virtual void initializeProblem (ProblemPtr_t problem);
599 
604 
611  std::string pathProjectorType_;
614 
616  std::string pathPlannerType_;
617 
620  private:
622  ConfigurationPtr_t initConf_;
624  Configurations_t goalConfigurations_;
626  std::string robotType_;
628  std::string configurationShooterType_;
630  std::string distanceType_;
632  std::string steeringMethodType_;
634  PathOptimizerTypes_t pathOptimizerTypes_;
635  PathOptimizers_t pathOptimizers_;
637  std::string pathValidationType_;
639  value_type pathValidationTolerance_;
640  // Config validation methods
641  ConfigValidationTypes_t configValidationTypes_;
643  ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
644  ObjectStdVector_t distanceObstacles_; // FIXME should be removed?
645  pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
646  pinocchio::DataPtr_t obstacleRData_; // Contains the frames
647  pinocchio::GeomModelPtr_t obstacleModel_;
648  pinocchio::GeomDataPtr_t obstacleData_;
649  // Tolerance for numerical constraint resolution
650  value_type errorThreshold_;
651  // Maximal number of iterations for numerical constraint resolution
652  size_type maxIterProjection_;
654  unsigned long int maxIterPathPlanning_;
656  double timeOutPathPlanning_;
658  segmentsMap_t passiveDofsMap_;
662  DistanceBetweenObjectsPtr_t distanceBetweenObjects_;
663 
664  void initProblem ();
665  }; // class ProblemSolver
666  } // namespace core
667 } // namespace hpp
668 
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
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