hpp-rbprm  4.10.0
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 
37  using core::Problem;
38  using core::Roadmap;
39  using core::RoadmapPtr_t;
40  using core::Path;
41  using core::PathPtr_t;
42  using core::Configuration_t;
43 
45 class DynamicPlanner : public core::BiRRTPlanner
46 {
47 public:
50  (const Problem& problem, const RoadmapPtr_t& roadmap);
52  static DynamicPlannerPtr_t create (const Problem& problem);
54  virtual void oneStep ();
57  virtual void tryConnectInitAndGoals();
58 
59  // we need both method, because smart_pointer inheritance is not implemented (compiler don't know that rbprmRoadmapPtr_t derive from RoadmapPtr_t).
60  virtual const core::RoadmapPtr_t& roadmap () const{
61  return roadmap_;
62  }
63 
64  virtual core::PathVectorPtr_t finishSolve (const core::PathVectorPtr_t& path);
65 
66 
67 protected:
69  DynamicPlanner (const Problem& problem, const RoadmapPtr_t& roadmap);
71  DynamicPlanner (const Problem& problem);
73  void init (const DynamicPlannerWkPtr_t& weak);
74 
80  void computeGIWC(const core::NodePtr_t x, core::ValidationReportPtr_t report);
81 
86  void computeGIWC(const core::NodePtr_t x, bool use_bestReport = true);
87 
88  core::PathPtr_t extendInternal (core::ConfigurationPtr_t& qProj_, const core::NodePtr_t& near,
89  const core::ConfigurationPtr_t& target, bool reverse=false);
90 
91  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);
92 
93  core::PathPtr_t extendParabola (const core::ConfigurationPtr_t &from,
94  const core::ConfigurationPtr_t& target, bool reverse);
95 
96 private:
97 
98 
99 
100  core::ConfigurationPtr_t qProj_;
101  DynamicPlannerWkPtr_t weakPtr_;
102  const core::RoadmapPtr_t roadmap_;
104  const SteeringMethodParabolaPtr_t smParabola_;
105  const RbPrmPathValidationPtr_t rbprmPathValidation_;
106  double sizeFootX_,sizeFootY_;
107  bool rectangularContact_;
108  bool tryJump_;
109  double mu_;
110 };
112 } // namespace core
113 } // namespace hpp
114 #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:45
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:60
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.