hpp-core  4.15.1
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 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
31 #define HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
32 
33 #include <hpp/core/path-planner.hh>
34 
35 namespace hpp {
36 namespace core {
37 namespace pathPlanner {
39 typedef shared_ptr<BiRrtStar> BiRrtStarPtr_t;
40 
44  public:
46 
49  static BiRrtStarPtr_t create(const ProblemConstPtr_t& problem);
54  const RoadmapPtr_t& roadmap);
55 
58  void startSolve();
60  void oneStep();
61 
62  protected:
65  BiRrtStar(const ProblemConstPtr_t& problem);
69  BiRrtStar(const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap);
71  void init(const BiRrtStarWkPtr_t& weak);
72 
73  private:
74  typedef std::map<NodePtr_t, EdgePtr_t> ParentMap_t;
75 
76  Configuration_t sample();
77 
79  value_type cost(NodePtr_t n);
80 
82  void cost(NodePtr_t n, value_type c);
83 
91  bool buildPath(const Configuration_t& q0, const Configuration_t& q1,
92  value_type maxLength, bool validatePath, PathPtr_t& result);
93 
94  bool extend(NodePtr_t target, ParentMap_t& parentMap, Configuration_t& q);
95 
96  bool connect(NodePtr_t cc, ParentMap_t& parentMap, const Configuration_t& q);
97 
98  bool improve(const Configuration_t& q);
99 
100  value_type gamma_;
102  value_type extendMaxLength_;
104  value_type minimalPathLength_;
105 
106  NodePtr_t roots_[2];
107 
109  std::vector<ParentMap_t> toRoot_;
110 
112  BiRrtStarWkPtr_t weak_;
113 }; // class BiRrtStar
114 } // namespace pathPlanner
115 } // namespace core
116 } // namespace hpp
117 
118 #endif // HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
Definition: node.hh:46
Definition: path-planner.hh:45
Definition: bi-rrt-star.hh:43
PathPlanner Parent_t
Definition: bi-rrt-star.hh:45
BiRrtStar(const ProblemConstPtr_t &problem, const RoadmapPtr_t &roadmap)
static BiRrtStarPtr_t createWithRoadmap(const ProblemConstPtr_t &problem, const RoadmapPtr_t &roadmap)
void oneStep()
One step of the algorithm.
void init(const BiRrtStarWkPtr_t &weak)
Store weak pointer to itself.
BiRrtStar(const ProblemConstPtr_t &problem)
static BiRrtStarPtr_t create(const ProblemConstPtr_t &problem)
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< BiRrtStar > BiRrtStarPtr_t
Definition: bi-rrt-star.hh:39
pinocchio::value_type value_type
Definition: fwd.hh:174
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:199
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:197
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
Definition: bi-rrt-planner.hh:35