hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
bi-rrt-star.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 CNRS
3 // Authors: Joseph Mirabel
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_PATH_PLANNER_BI_RRT_STAR_HH
20 # define HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
21 
22 # include <hpp/core/path-planner.hh>
23 
24 namespace hpp {
25  namespace core {
26  namespace pathPlanner {
27  HPP_PREDEF_CLASS (BiRrtStar);
28  typedef shared_ptr <BiRrtStar> BiRrtStarPtr_t;
29 
33  {
34  public:
36 
39  static BiRrtStarPtr_t create (const ProblemConstPtr_t& problem);
43  static BiRrtStarPtr_t createWithRoadmap
44  (const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap);
45 
48  void startSolve ();
50  void oneStep ();
51 
52  protected:
55  BiRrtStar (const ProblemConstPtr_t& problem);
59  BiRrtStar (const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap);
61  void init (const BiRrtStarWkPtr_t& weak);
62 
63  private:
64  typedef std::map<NodePtr_t, EdgePtr_t> ParentMap_t;
65 
66  Configuration_t sample ();
67 
69  value_type cost(NodePtr_t n);
70 
72  void cost(NodePtr_t n, value_type c);
73 
78  PathPtr_t buildPath(const Configuration_t& q0, const Configuration_t& q1,
79  value_type maxLength, bool validatePath);
80 
81  bool extend (NodePtr_t target, ParentMap_t& parentMap, Configuration_t& q);
82 
83  bool connect (NodePtr_t cc, ParentMap_t& parentMap, const Configuration_t& q);
84 
85  bool improve (const Configuration_t& q);
86 
87  value_type gamma_;
89  value_type extendMaxLength_;
91  value_type minimalPathLength_;
92 
93  NodePtr_t roots_[2];
94 
96  std::vector<ParentMap_t> toRoot_;
97 
99  BiRrtStarWkPtr_t weak_;
100  }; // class BiRrtStar
101  } // namespace pathPlanner
102  } // namespace core
103 } // namespace hpp
104 
105 #endif // HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
Definition: bi-rrt-planner.hh:24
Definition: path-planner.hh:34
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
shared_ptr< BiRrtStar > BiRrtStarPtr_t
Definition: bi-rrt-star.hh:28
Definition: node.hh:35
pinocchio::value_type value_type
Definition: fwd.hh:157
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:182
Definition: bi-rrt-star.hh:32
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
PathPlanner Parent_t
Definition: bi-rrt-star.hh:35
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170