Loading...
Searching...
No Matches
BKPIECE1.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: Ioan Sucan */
36
37#include "ompl/geometric/planners/kpiece/BKPIECE1.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <cassert>
41
42ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si)
43 : base::Planner(si, "BKPIECE1")
44 , dStart_([this](Motion *m) { freeMotion(m); })
45 , dGoal_([this](Motion *m) { freeMotion(m); })
46{
47 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
48
49 Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000.");
50 Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction,
51 "0.:.05:1.");
52 Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor,
54 Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction,
56}
57
58ompl::geometric::BKPIECE1::~BKPIECE1() = default;
59
61{
62 Planner::setup();
66
67 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
68 throw Exception("Failed expansion cell score factor must be in the range (0,1]");
69 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
70 throw Exception("The minimum valid path fraction must be in the range (0,1]");
71
72 dStart_.setDimension(projectionEvaluator_->getDimension());
73 dGoal_.setDimension(projectionEvaluator_->getDimension());
74}
75
77{
79 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
80
81 if (goal == nullptr)
82 {
83 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
85 }
86
88
89 while (const base::State *st = pis_.nextStart())
90 {
91 auto *motion = new Motion(si_);
92 si_->copyState(motion->state, st);
93 motion->root = motion->state;
94 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
95 dStart_.addMotion(motion, xcoord);
96 }
97
98 if (dStart_.getMotionCount() == 0)
99 {
100 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
102 }
103
104 if (!goal->couldSample())
105 {
106 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
108 }
109
110 if (!sampler_)
111 sampler_ = si_->allocValidStateSampler();
112
113 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
114 (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
115
116 std::vector<Motion *> solution;
117 base::State *xstate = si_->allocState();
118 bool startTree = true;
119 bool solved = false;
120
121 while (!ptc)
122 {
123 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
124 startTree = !startTree;
125 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
126 disc.countIteration();
127
128 // if we have not sampled too many goals already
129 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
130 {
131 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
132 if (st != nullptr)
133 {
134 auto *motion = new Motion(si_);
135 si_->copyState(motion->state, st);
136 motion->root = motion->state;
137 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
138 dGoal_.addMotion(motion, xcoord);
139 }
140 if (dGoal_.getMotionCount() == 0)
141 {
142 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
143 break;
144 }
145 }
146
147 Discretization<Motion>::Cell *ecell = nullptr;
148 Motion *existing = nullptr;
149 disc.selectMotion(existing, ecell);
150 assert(existing);
151 if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
152 {
153 std::pair<base::State *, double> fail(xstate, 0.0);
154 bool keep = si_->checkMotion(existing->state, xstate, fail);
155 if (!keep && fail.second > minValidPathFraction_)
156 keep = true;
157
158 if (keep)
159 {
160 /* create a motion */
161 auto *motion = new Motion(si_);
162 si_->copyState(motion->state, xstate);
163 motion->root = existing->root;
164 motion->parent = existing;
165
166 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
167 disc.addMotion(motion, xcoord);
168
169 Discretization<Motion>::Cell *cellC = otherDisc.getGrid().getCell(xcoord);
170
171 if ((cellC != nullptr) && !cellC->data->motions.empty())
172 {
173 Motion *connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
174
175 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
176 startTree ? motion->root : connectOther->root) &&
177 si_->checkMotion(motion->state, connectOther->state))
178 {
179 if (startTree)
180 connectionPoint_ = std::make_pair(connectOther->state, motion->state);
181 else
182 connectionPoint_ = std::make_pair(motion->state, connectOther->state);
183
184 /* extract the motions and put them in solution vector */
185
186 std::vector<Motion *> mpath1;
187 while (motion != nullptr)
188 {
189 mpath1.push_back(motion);
190 motion = motion->parent;
191 }
192
193 std::vector<Motion *> mpath2;
194 while (connectOther != nullptr)
195 {
196 mpath2.push_back(connectOther);
197 connectOther = connectOther->parent;
198 }
199
200 if (startTree)
201 mpath1.swap(mpath2);
202
203 auto path(std::make_shared<PathGeometric>(si_));
204 path->getStates().reserve(mpath1.size() + mpath2.size());
205 for (int i = mpath1.size() - 1; i >= 0; --i)
206 path->append(mpath1[i]->state);
207 for (auto &i : mpath2)
208 path->append(i->state);
209
210 pdef_->addSolutionPath(path, false, 0.0, getName());
211 solved = true;
212 break;
213 }
214 }
215 }
216 else
217 ecell->data->score *= failedExpansionScoreFactor_;
218 }
219 else
220 ecell->data->score *= failedExpansionScoreFactor_;
221 disc.updateCell(ecell);
222 }
223
224 si_->freeState(xstate);
225
226 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
227 "boundary))",
228 getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
229 dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
230 dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
231
233}
234
236{
237 if (motion->state != nullptr)
238 si_->freeState(motion->state);
239 delete motion;
240}
241
243{
244 Planner::clear();
245
246 sampler_.reset();
247 dStart_.clear();
248 dGoal_.clear();
249 connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
250}
251
253{
254 Planner::getPlannerData(data);
255 dStart_.getPlannerData(data, 1, true, nullptr);
256 dGoal_.getPlannerData(data, 2, false, nullptr);
257
258 // Insert the edge connecting the two trees
259 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
260}
The exception type for ompl.
Definition Exception.h:47
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition GridN.h:123
Abstract definition of a goal region that can be sampled.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
Definition of an abstract state.
Definition State.h:50
Representation of a motion for this algorithm.
Definition BKPIECE1.h:179
Motion * parent
The parent motion in the exploration tree.
Definition BKPIECE1.h:197
base::State * state
The state contained by this motion.
Definition BKPIECE1.h:194
const base::State * root
The root state (start state) that leads to this motion.
Definition BKPIECE1.h:191
void setFailedExpansionCellScoreFactor(double factor)
When extending a motion from a cell, the extension can be successful or it can fail....
Definition BKPIECE1.h:140
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 BKPIECE1.cpp:252
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition BKPIECE1.h:228
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition BKPIECE1.h:158
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition BKPIECE1.h:164
BKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition BKPIECE1.cpp:42
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 BKPIECE1.cpp:76
RNG rng_
The random number generator.
Definition BKPIECE1.h:231
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition BKPIECE1.h:234
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 BKPIECE1.h:123
Discretization< Motion > dGoal_
The goal tree.
Definition BKPIECE1.h:213
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition BKPIECE1.h:131
base::ValidStateSamplerPtr sampler_
The employed state sampler.
Definition BKPIECE1.h:204
double failedExpansionScoreFactor_
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
Definition BKPIECE1.h:218
double minValidPathFraction_
When extending a motion, the planner can decide to keep the first valid part of it,...
Definition BKPIECE1.h:225
Discretization< Motion > dStart_
The start tree.
Definition BKPIECE1.h:210
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BKPIECE1.cpp:60
void setRange(double distance)
Set the range the planner is supposed to use.
Definition BKPIECE1.h:106
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BKPIECE1.cpp:242
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition BKPIECE1.h:207
double getRange() const
Get the range the planner is using.
Definition BKPIECE1.h:112
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition BKPIECE1.cpp:235
double getFailedExpansionCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed.
Definition BKPIECE1.h:147
One-level discretization used for KPIECE.
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
unsigned int addMotion(Motion *motion, const Coord &coord, double dist=0.0)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
typename Grid::Cell Cell
The datatype for the maintained grid cells.
void selectMotion(Motion *&smotion, Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.