hpp-rbprm  4.12.0
Implementation of RB-PRM planner using hpp.
parabola-planner.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Fernbach Pierre
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_PARABOLA_PLANNER_HH
20 #define HPP_RBPRM_PARABOLA_PLANNER_HH
21 
22 #include <hpp/core/path-planner.hh>
23 #include <hpp/core/steering-method.hh>
24 #include <boost/tuple/tuple.hpp>
27 
28 namespace hpp {
29 namespace rbprm {
32 
33 // forward declaration of class Planner
34 HPP_PREDEF_CLASS(ParabolaPlanner);
35 // Planner objects are manipulated only via shared pointers
36 typedef std::shared_ptr<ParabolaPlanner> ParabolaPlannerPtr_t;
37 typedef boost::tuple<core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t> DelayedEdge_t;
38 typedef std::vector<DelayedEdge_t> DelayedEdges_t;
39 
41 class ParabolaPlanner : public core::PathPlanner {
42  public:
44  static ParabolaPlannerPtr_t createWithRoadmap(const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
46  static ParabolaPlannerPtr_t create(const core::Problem& problem);
48  virtual void oneStep();
49 
50  // for debugging
51  // virtual core::PathVectorPtr_t solve ();
52 
53  virtual void startSolve();
54 
55  // disabled during testing
56  virtual void tryDirectPath();
57 
58  // This method call SteeringMethodParabola, but we don't try to connect two confuration, instead we shoot a random
59  // alpha0 and V0 valide for the initiale configuration and then compute the final point. Then we check for collision
60  // (for the trunk) and we check if the final point is in a valide configuration (trunk not in collision but limbs in
61  // accessible contact zone). (Not anymore ) If this is true we do a reverse collision check until we find the first
62  // valide configuration, then we check for the friction cone and impact velocity constraint.(Not anymore : can't find
63  // normal after this)
64 
72  core::PathPtr_t computeRandomParabola(core::NodePtr_t x_start, core::ConfigurationPtr_t q_target,
73  DelayedEdges_t& delayedEdges);
74 
76  void configurationShooter(const core::ConfigurationShooterPtr_t& shooter);
77 
78  // we need both method, because smart_pointer inheritance is not implemented (compiler don't know that
79  // rbprmRoadmapPtr_t derive from RoadmapPtr_t).
80  virtual const core::RoadmapPtr_t& roadmap() const { return roadmap_; }
81 
82  const core::RbprmRoadmapPtr_t& rbprmRoadmap() const { return rbRoadmap_; }
83 
84  protected:
86  ParabolaPlanner(const core::Problem& problem, const core::RoadmapPtr_t& roadmap);
88  ParabolaPlanner(const core::Problem& problem);
90  void init(const ParabolaPlannerWkPtr_t& weak);
94  virtual core::PathPtr_t extend(const core::NodePtr_t& near, const core::ConfigurationPtr_t& target);
95  virtual core::PathPtr_t extendParabola(const core::NodePtr_t& near, const core::ConfigurationPtr_t& target);
96 
97  private:
103  void computeGIWC(const core::RbprmNodePtr_t x, core::ValidationReportPtr_t report);
104 
110  void computeGIWC(const core::RbprmNodePtr_t x);
111 
112  void computeGIWC(const core::NodePtr_t x) { computeGIWC(static_cast<core::RbprmNodePtr_t>(x)); }
113 
114  void computeGIWC(const core::NodePtr_t node, core::ValidationReportPtr_t report) {
115  computeGIWC(static_cast<core::RbprmNodePtr_t>(node), report);
116  }
117 
118  core::ConfigurationShooterPtr_t configurationShooter_;
119  mutable core::Configuration_t qProj_;
120  ParabolaPlannerWkPtr_t weakPtr_;
121  SteeringMethodParabolaPtr_t smParabola_;
122  const core::RbprmRoadmapPtr_t rbRoadmap_;
123  const core::RoadmapPtr_t roadmap_;
124 };
126 } // namespace rbprm
127 } // namespace hpp
128 #endif // HPP_CORE_DIFFUSING_PLANNER_HH
Definition: algorithm.hh:27
ParabolaPlanner(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Constructor.
std::vector< DelayedEdge_t > DelayedEdges_t
Definition: parabola-planner.hh:38
Definition: rbprm-node.hh:22
virtual core::PathPtr_t extend(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
static ParabolaPlannerPtr_t createWithRoadmap(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Return shared pointer to new object.
std::shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
std::shared_ptr< ParabolaPlanner > ParabolaPlannerPtr_t
Definition: parabola-planner.hh:36
boost::tuple< core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t > DelayedEdge_t
Definition: parabola-planner.hh:37
virtual void oneStep()
One step of extension.
HPP_PREDEF_CLASS(RbPrmFullBody)
virtual core::PathPtr_t extendParabola(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
void init(const ParabolaPlannerWkPtr_t &weak)
Store weak pointer to itself.
void configurationShooter(const core::ConfigurationShooterPtr_t &shooter)
Set configuration shooter.
virtual const core::RoadmapPtr_t & roadmap() const
Definition: parabola-planner.hh:80
std::shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
core::PathPtr_t computeRandomParabola(core::NodePtr_t x_start, core::ConfigurationPtr_t q_target, DelayedEdges_t &delayedEdges)
computeRandomParabola
Generic implementation of RRT algorithm.
Definition: parabola-planner.hh:41
virtual void tryDirectPath()
const core::RbprmRoadmapPtr_t & rbprmRoadmap() const
Definition: parabola-planner.hh:82
static ParabolaPlannerPtr_t create(const core::Problem &problem)
Return shared pointer to new object.