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...
Definition: RandomNumbers.h:58
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
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.
Definition: LBKPIECE1.cpp:273
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...
Definition: LBKPIECE1.cpp:310
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...
Definition: LBKPIECE1.cpp:320
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
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...
Definition: LBKPIECE1.cpp:228
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: LBKPIECE1.h:101
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:303
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()
Definition: PlannerStatus.h:49