Loading...
Searching...
No Matches
LBKPIECE1.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_LBKPIECE1_
38#define OMPL_GEOMETRIC_PLANNERS_KPIECE_LBKPIECE1_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/geometric/planners/kpiece/Discretization.h"
42
43namespace ompl
44{
45 namespace geometric
46 {
78 class LBKPIECE1 : public base::Planner
79 {
80 public:
82 LBKPIECE1(const base::SpaceInformationPtr &si);
83
84 ~LBKPIECE1() override;
85
88 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
89 {
90 projectionEvaluator_ = projectionEvaluator;
91 }
92
95 void setProjectionEvaluator(const std::string &name)
96 {
97 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
98 }
99
101 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
102 {
104 }
105
111 void setRange(double distance)
112 {
113 maxDistance_ = distance;
114 }
115
117 double getRange() const
118 {
119 return maxDistance_;
120 }
127 void setBorderFraction(double bp)
128 {
129 dStart_.setBorderFraction(bp);
130 dGoal_.setBorderFraction(bp);
131 }
132
135 double getBorderFraction() const
136 {
137 return dStart_.getBorderFraction();
138 }
139
146 void setMinValidPathFraction(double fraction)
147 {
148 minValidPathFraction_ = fraction;
149 }
150
153 {
155 }
156
157 void setup() override;
158
160 void clear() override;
161
162 void getPlannerData(base::PlannerData &data) const override;
163
164 protected:
166 class Motion
167 {
168 public:
169 Motion() = default;
170
172 Motion(const base::SpaceInformationPtr &si)
173 : state(si->allocState())
174 {
175 }
176
177 ~Motion() = default;
178
180 const base::State *root{nullptr};
181
184
186 Motion *parent{nullptr};
187
189 bool valid{false};
190
192 std::vector<Motion *> children;
193 };
194
196 void freeMotion(Motion *motion);
197
199 void removeMotion(Discretization<Motion> &disc, Motion *motion);
200
206 bool isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp);
207
209 base::StateSamplerPtr sampler_;
210
212 base::ProjectionEvaluatorPtr projectionEvaluator_;
213
216
219
226
228 double maxDistance_{0.};
229
232
234 std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr};
235 };
236 }
237}
238
239#endif
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
One-level discretization used for KPIECE.
Representation of a motion for this algorithm.
Definition LBKPIECE1.h:167
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition LBKPIECE1.h:192
Motion * parent
The parent motion in the exploration tree.
Definition LBKPIECE1.h:186
const base::State * root
The root state (start state) that leads to this motion.
Definition LBKPIECE1.h:180
base::State * state
The state contained by this motion.
Definition LBKPIECE1.h:183
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition LBKPIECE1.h:172
bool valid
Flag indicating whether this motion has been checked for validity.
Definition LBKPIECE1.h:189
Lazy Bi-directional KPIECE with one level of discretization.
Definition LBKPIECE1.h:79
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition LBKPIECE1.h:135
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition LBKPIECE1.h:212
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition LBKPIECE1.h:228
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition LBKPIECE1.h:146
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
double getRange() const
Get the range the planner is using.
Definition LBKPIECE1.h:117
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition LBKPIECE1.h:234
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
double minValidPathFraction_
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition LBKPIECE1.h:225
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition LBKPIECE1.h:152
RNG rng_
The random number generator.
Definition LBKPIECE1.h:231
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
Definition LBKPIECE1.h:127
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition LBKPIECE1.cpp:58
void setRange(double distance)
Set the range the planner is supposed to use.
Definition LBKPIECE1.h:111
LBKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition LBKPIECE1.cpp:42
bool isPathValid(Discretization< Motion > &disc, Motion *motion, base::State *temp)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition LBKPIECE1.h:101
void freeMotion(Motion *motion)
Free the memory for a motion.
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition LBKPIECE1.h:88
base::StateSamplerPtr sampler_
The employed state sampler.
Definition LBKPIECE1.h:209
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition LBKPIECE1.h:95
Discretization< Motion > dStart_
The start tree.
Definition LBKPIECE1.h:215
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 LBKPIECE1.cpp:72
Discretization< Motion > dGoal_
The goal tree.
Definition LBKPIECE1.h:218
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()