Loading...
Searching...
No Matches
BiEST.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ryan Luna */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_EST_BIEST_
38#define OMPL_GEOMETRIC_PLANNERS_EST_BIEST_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/datastructures/NearestNeighbors.h"
42#include "ompl/datastructures/PDF.h"
43#include <vector>
44
45namespace ompl
46{
47 namespace geometric
48 {
65 class BiEST : public base::Planner
66 {
67 public:
69 BiEST(const base::SpaceInformationPtr &si);
70
71 ~BiEST() override;
72
74
75 void clear() override;
76
82 void setRange(double distance)
83 {
84 maxDistance_ = distance;
85
86 // Make the neighborhood radius smaller than sampling range to
87 // keep probabilities relatively high for rejection sampling
89 }
90
92 double getRange() const
93 {
94 return maxDistance_;
95 }
96
97 void setup() override;
98
99 void getPlannerData(base::PlannerData &data) const override;
100
101 protected:
103 class Motion
104 {
105 public:
106 Motion() = default;
107
109 Motion(const base::SpaceInformationPtr &si)
110 : state(si->allocState())
111 {
112 }
113
114 ~Motion() = default;
115
118
120 Motion *parent{nullptr};
121
124
126 const base::State *root{nullptr};
127 };
128
130 double distanceFunction(const Motion *a, const Motion *b) const
131 {
132 return si_->distance(a->state, b->state);
133 }
134
136 std::shared_ptr<NearestNeighbors<Motion *>> nnStart_;
137 std::shared_ptr<NearestNeighbors<Motion *>> nnGoal_;
138
140 std::vector<Motion *> startMotions_;
141 std::vector<Motion *> goalMotions_;
142
145 PDF<Motion *> goalPdf_;
146
148 void freeMemory();
149
151 void addMotion(Motion *motion, std::vector<Motion *> &motions, PDF<Motion *> &pdf,
152 const std::shared_ptr<NearestNeighbors<Motion *>> &nn,
153 const std::vector<Motion *> &neighbors);
154
156 base::ValidStateSamplerPtr sampler_;
157
159 double maxDistance_{0.0};
160
163
166
168 std::pair<base::State *, base::State *> connectionPoint_{nullptr,nullptr};
169 };
170 }
171}
172
173#endif
Abstract representation of a container that can perform nearest neighbors queries.
A class that will hold data contained in the PDF.
Definition PDF.h:53
A container that supports probabilistic sampling over weighted data.
Definition PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Definition of an abstract state.
Definition State.h:50
The definition of a motion.
Definition BiEST.h:104
base::State * state
The state contained by the motion.
Definition BiEST.h:117
const base::State * root
The root node of the tree this motion is in.
Definition BiEST.h:126
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition BiEST.h:109
Motion * parent
The parent motion in the exploration tree.
Definition BiEST.h:120
PDF< Motion * >::Element * element
A pointer to the corresponding element in the probability distribution function.
Definition BiEST.h:123
Bi-directional Expansive Space Trees.
Definition BiEST.h:66
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition BiEST.h:156
PDF< Motion * > startPdf_
The probability distribution function over states in each tree.
Definition BiEST.h:144
void addMotion(Motion *motion, std::vector< Motion * > &motions, PDF< Motion * > &pdf, const std::shared_ptr< NearestNeighbors< Motion * > > &nn, const std::vector< Motion * > &neighbors)
Add a motion to the exploration tree.
Definition BiEST.cpp:279
void freeMemory()
Free the memory allocated by this planner.
Definition BiEST.cpp:100
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BiEST.cpp:56
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition BiEST.cpp:117
double getRange() const
Get the range the planner is using.
Definition BiEST.h:92
double nbrhoodRadius_
The radius considered for neighborhood.
Definition BiEST.h:162
std::shared_ptr< NearestNeighbors< Motion * > > nnStart_
A nearest-neighbors datastructure containing the tree of motions.
Definition BiEST.h:136
RNG rng_
The random number generator.
Definition BiEST.h:165
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BiEST.cpp:81
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition BiEST.h:159
std::vector< Motion * > startMotions_
The set of all states in the start tree.
Definition BiEST.h:140
void setRange(double distance)
Set the range the planner is supposed to use.
Definition BiEST.h:82
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition BiEST.cpp:296
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition BiEST.h:168
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition BiEST.h:130
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()