hpp-core 5.0.0
Implement basic classes for canonical path planning for kinematic chains.
Loading...
Searching...
No Matches
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
34
35namespace hpp {
36namespace core {
37namespace pathPlanner {
39typedef 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:
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:88
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:107
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
Definition: bi-rrt-planner.hh:35