hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
dynamic-planner.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 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_RBPRM_DYNAMIC_PLANNER_HH
20 #define HPP_RBPRM_DYNAMIC_PLANNER_HH
21 
22 #include <hpp/core/bi-rrt-planner.hh>
26 
27 namespace hpp {
28 namespace rbprm {
31 // forward declaration of class Planner
33 // Planner objects are manipulated only via shared pointers
34 typedef shared_ptr<DynamicPlanner> DynamicPlannerPtr_t;
35 
36 using core::Configuration_t;
37 using core::Path;
38 using core::PathPtr_t;
39 using core::Problem;
40 using core::Roadmap;
41 using core::RoadmapPtr_t;
42 
44 class DynamicPlanner : public core::BiRRTPlanner {
45  public:
47  static DynamicPlannerPtr_t createWithRoadmap(core::ProblemConstPtr_t problem,
48  const RoadmapPtr_t& roadmap);
50  static DynamicPlannerPtr_t create(core::ProblemConstPtr_t problem);
52  virtual void oneStep();
55  virtual void tryConnectInitAndGoals();
56 
57  // we need both method, because smart_pointer inheritance is not implemented
58  // (compiler don't know that rbprmRoadmapPtr_t derive from RoadmapPtr_t).
59  virtual const core::RoadmapPtr_t& roadmap() const { return roadmap_; }
60 
61  virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t& path);
62 
63  protected:
65  DynamicPlanner(core::ProblemConstPtr_t problem, const RoadmapPtr_t& roadmap);
67  DynamicPlanner(core::ProblemConstPtr_t problem);
69  void init(const DynamicPlannerWkPtr_t& weak);
70 
77  void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report);
78 
84  void computeGIWC(const core::NodePtr_t x, bool use_bestReport = true);
85 
86  core::PathPtr_t extendInternal(core::ConfigurationPtr_t& qProj_,
87  const core::NodePtr_t& near,
88  const core::ConfigurationPtr_t& target,
89  bool reverse = false);
90 
91  bool tryParabolaPath(const core::NodePtr_t& near,
92  core::ConfigurationPtr_t q_jump,
93  const core::ConfigurationPtr_t& target, bool reverse,
94  core::NodePtr_t& x_jump, core::NodePtr_t& nodeReached,
95  core::PathPtr_t& kinoPath, core::PathPtr_t& paraPath);
96 
97  core::PathPtr_t extendParabola(const core::ConfigurationPtr_t& from,
98  const core::ConfigurationPtr_t& target,
99  bool reverse);
100 
101  private:
102  core::ConfigurationPtr_t qProj_;
103  DynamicPlannerWkPtr_t weakPtr_;
104  const core::RoadmapPtr_t roadmap_;
106  const SteeringMethodParabolaPtr_t smParabola_;
107  const RbPrmPathValidationPtr_t rbprmPathValidation_;
108  double sizeFootX_, sizeFootY_;
109  bool rectangularContact_;
110  bool tryJump_;
111  double mu_;
112 };
114 } // namespace rbprm
115 } // namespace hpp
116 #endif // HPP_RBPRM_DYNAMIC_PLANNER_HH
Generic implementation of RRT algorithm.
Definition: dynamic-planner.hh:44
void computeGIWC(const core::NodePtr_t x, bool use_bestReport=true)
computeGIWC compute the GIWC for the node configuration and fill the node attribut,...
core::PathPtr_t extendInternal(core::ConfigurationPtr_t &qProj_, const core::NodePtr_t &near, const core::ConfigurationPtr_t &target, bool reverse=false)
DynamicPlanner(core::ProblemConstPtr_t problem, const RoadmapPtr_t &roadmap)
Constructor.
core::PathPtr_t extendParabola(const core::ConfigurationPtr_t &from, const core::ConfigurationPtr_t &target, bool reverse)
virtual const core::RoadmapPtr_t & roadmap() const
Definition: dynamic-planner.hh:59
bool tryParabolaPath(const core::NodePtr_t &near, core::ConfigurationPtr_t q_jump, const core::ConfigurationPtr_t &target, bool reverse, core::NodePtr_t &x_jump, core::NodePtr_t &nodeReached, core::PathPtr_t &kinoPath, core::PathPtr_t &paraPath)
static DynamicPlannerPtr_t createWithRoadmap(core::ProblemConstPtr_t problem, const RoadmapPtr_t &roadmap)
Return shared pointer to new object.
virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t &path)
virtual void oneStep()
One step of extension.
DynamicPlanner(core::ProblemConstPtr_t problem)
Constructor with roadmap.
void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report)
computeGIWC compute the GIWC for the node configuration and fill the node attribut
static DynamicPlannerPtr_t create(core::ProblemConstPtr_t problem)
Return shared pointer to new object.
virtual void tryConnectInitAndGoals()
void init(const DynamicPlannerWkPtr_t &weak)
Store weak pointer to itself.
shared_ptr< DynamicPlanner > DynamicPlannerPtr_t
Definition: dynamic-planner.hh:34
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< RbPrmPathValidation > RbPrmPathValidationPtr_t
Definition: rbprm-path-validation.hh:31
shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
Definition: algorithm.hh:26