hpp-rbprm  4.10.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
32 HPP_PREDEF_CLASS(DynamicPlanner);
33 // Planner objects are manipulated only via shared pointers
34 typedef boost::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(const Problem& problem, const RoadmapPtr_t& roadmap);
49  static DynamicPlannerPtr_t create(const Problem& problem);
51  virtual void oneStep();
54  virtual void tryConnectInitAndGoals();
55 
56  // we need both method, because smart_pointer inheritance is not implemented (compiler don't know that
57  // rbprmRoadmapPtr_t derive from RoadmapPtr_t).
58  virtual const core::RoadmapPtr_t& roadmap() const { return roadmap_; }
59 
60  virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t& path);
61 
62  protected:
64  DynamicPlanner(const Problem& problem, const RoadmapPtr_t& roadmap);
66  DynamicPlanner(const Problem& problem);
68  void init(const DynamicPlannerWkPtr_t& weak);
69 
75  void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report);
76 
82  void computeGIWC(const core::NodePtr_t x, bool use_bestReport = true);
83 
84  core::PathPtr_t extendInternal(core::ConfigurationPtr_t& qProj_, const core::NodePtr_t& near,
85  const core::ConfigurationPtr_t& target, bool reverse = false);
86 
87  bool tryParabolaPath(const core::NodePtr_t& near, core::ConfigurationPtr_t q_jump,
88  const core::ConfigurationPtr_t& target, bool reverse, core::NodePtr_t& x_jump,
89  core::NodePtr_t& nodeReached, core::PathPtr_t& kinoPath, core::PathPtr_t& paraPath);
90 
91  core::PathPtr_t extendParabola(const core::ConfigurationPtr_t& from, const core::ConfigurationPtr_t& target,
92  bool reverse);
93 
94  private:
95  core::ConfigurationPtr_t qProj_;
96  DynamicPlannerWkPtr_t weakPtr_;
97  const core::RoadmapPtr_t roadmap_;
99  const SteeringMethodParabolaPtr_t smParabola_;
100  const RbPrmPathValidationPtr_t rbprmPathValidation_;
101  double sizeFootX_, sizeFootY_;
102  bool rectangularContact_;
103  bool tryJump_;
104  double mu_;
105 };
107 } // namespace rbprm
108 } // namespace hpp
109 #endif // HPP_RBPRM_DYNAMIC_PLANNER_HH
rbprm-path-validation.hh
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::DynamicPlanner::finishSolve
virtual core::PathVectorPtr_t finishSolve(const core::PathVectorPtr_t &path)
hpp::rbprm::DynamicPlanner::computeGIWC
void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report)
computeGIWC compute the GIWC for the node configuration and fill the node attribut
hpp::rbprm::DynamicPlanner::tryConnectInitAndGoals
virtual void tryConnectInitAndGoals()
hpp::rbprm::DynamicPlanner
Generic implementation of RRT algorithm.
Definition: dynamic-planner.hh:44
hpp::rbprm::DynamicPlanner::tryParabolaPath
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)
hpp::rbprm::DynamicPlanner::init
void init(const DynamicPlannerWkPtr_t &weak)
Store weak pointer to itself.
hpp::rbprm::RbPrmPathValidationPtr_t
boost::shared_ptr< RbPrmPathValidation > RbPrmPathValidationPtr_t
Definition: rbprm-path-validation.hh:31
hpp::rbprm::DynamicPlanner::roadmap
virtual const core::RoadmapPtr_t & roadmap() const
Definition: dynamic-planner.hh:58
hpp::rbprm::SteeringMethodParabolaPtr_t
boost::shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
hpp::rbprm::SteeringMethodKinodynamicPtr_t
boost::shared_ptr< SteeringMethodKinodynamic > SteeringMethodKinodynamicPtr_t
Definition: rbprm-steering-kinodynamic.hh:33
hpp::rbprm::DynamicPlanner::extendParabola
core::PathPtr_t extendParabola(const core::ConfigurationPtr_t &from, const core::ConfigurationPtr_t &target, bool reverse)
hpp::rbprm::DynamicPlanner::DynamicPlanner
DynamicPlanner(const Problem &problem, const RoadmapPtr_t &roadmap)
Constructor.
hpp
Definition: algorithm.hh:27
hpp::rbprm::DynamicPlanner::createWithRoadmap
static DynamicPlannerPtr_t createWithRoadmap(const Problem &problem, const RoadmapPtr_t &roadmap)
Return shared pointer to new object.
rbprm-steering-kinodynamic.hh
steering-method-parabola.hh
hpp::rbprm::DynamicPlanner::oneStep
virtual void oneStep()
One step of extension.
hpp::rbprm::DynamicPlannerPtr_t
boost::shared_ptr< DynamicPlanner > DynamicPlannerPtr_t
Definition: dynamic-planner.hh:34
hpp::rbprm::DynamicPlanner::extendInternal
core::PathPtr_t extendInternal(core::ConfigurationPtr_t &qProj_, const core::NodePtr_t &near, const core::ConfigurationPtr_t &target, bool reverse=false)
hpp::rbprm::DynamicPlanner::create
static DynamicPlannerPtr_t create(const Problem &problem)
Return shared pointer to new object.