hpp-core: include/hpp/core/path-planner/bi-rrt-star.hh Source File

hpp-core 9.0.2 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
33#include <hpp/core/path-planner.hh>
34
35namespace hpp {
36namespace core {
37namespace pathPlanner {
38HPP_PREDEF_CLASS(BiRrtStar);
39 typedef shared_ptr<BiRrtStar> BiRrtStarPtr_t;
40
43
46 class HPP_CORE_DLLAPI BiRrtStar : public PathPlanner {
47 public:
48 typedef PathPlanner Parent_t;
49
52 static BiRrtStarPtr_t create(const ProblemConstPtr_t& problem);
56 static BiRrtStarPtr_t createWithRoadmap(const ProblemConstPtr_t& problem,
57 const RoadmapPtr_t& roadmap);
58
61 void startSolve();
63 void oneStep();
64
65 protected:
68 BiRrtStar(const ProblemConstPtr_t& problem);
72 BiRrtStar(const ProblemConstPtr_t& problem, const RoadmapPtr_t& roadmap);
74 void init(const BiRrtStarWkPtr_t& weak);
75
76 private:
77 typedef std::map<NodePtr_t, EdgePtr_t> ParentMap_t;
78
79 Configuration_t sample();
80
82 value_type cost(NodePtr_t n);
83
85 void cost(NodePtr_t n, value_type c);
86
94 bool buildPath(const Configuration_t& q0, const Configuration_t& q1,
95 value_type maxLength, bool validatePath, PathPtr_t& result);
96
97 bool extend(NodePtr_t target, ParentMap_t& parentMap, Configuration_t& q);
98
99 bool connect(NodePtr_t cc, ParentMap_t& parentMap, const Configuration_t& q);
100
101 bool improve(const Configuration_t& q);
102
103 value_type gamma_;
105 value_type extendMaxLength_;
107 value_type minimalPathLength_;
108
109 NodePtr_t roots_[2];
110
112 std::vector<ParentMap_t> toRoot_;
113
115 BiRrtStarWkPtr_t weak_;
116}; // class BiRrtStar
117
119
120} // namespace pathPlanner
121} // namespace core
122} // namespace hpp
123
124#endif // HPP_CORE_PATH_PLANNER_BI_RRT_STAR_HH
hpp::core::Node
Definition node.hh:46
hpp::core::PathPlanner
Definition path-planner.hh:45
hpp::core::pathPlanner::BiRrtStar
Definition bi-rrt-star.hh:46
hpp::core::pathPlanner::BiRrtStar::Parent_t
PathPlanner Parent_t
Definition bi-rrt-star.hh:48
hpp::core::pathPlanner::BiRrtStar::startSolve
void startSolve()
hpp::core::pathPlanner::BiRrtStar::BiRrtStar
BiRrtStar(const ProblemConstPtr_t &problem, const RoadmapPtr_t &roadmap)
hpp::core::pathPlanner::BiRrtStar::createWithRoadmap
static BiRrtStarPtr_t createWithRoadmap(const ProblemConstPtr_t &problem, const RoadmapPtr_t &roadmap)
hpp::core::pathPlanner::BiRrtStar::oneStep
void oneStep()
One step of the algorithm.
hpp::core::pathPlanner::BiRrtStar::init
void init(const BiRrtStarWkPtr_t &weak)
Store weak pointer to itself.
hpp::core::pathPlanner::BiRrtStar::BiRrtStar
BiRrtStar(const ProblemConstPtr_t &problem)
hpp::core::pathPlanner::BiRrtStar::create
static BiRrtStarPtr_t create(const ProblemConstPtr_t &problem)
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
hpp
hpp::core::pathPlanner::BiRrtStarPtr_t
shared_ptr< BiRrtStar > BiRrtStarPtr_t
Definition bi-rrt-star.hh:39
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::RoadmapPtr_t
shared_ptr< Roadmap > RoadmapPtr_t
Definition fwd.hh:199
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
path-planner.hh