Loading...
Searching...
No Matches
LBKPIECE1.cpp
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#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <cassert>
41
42ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si)
43 : base::Planner(si, "LBKPIECE1")
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, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
50 Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction,
51 "0.:.05:1.");
52 Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction,
54}
55
56ompl::geometric::LBKPIECE1::~LBKPIECE1() = default;
57
59{
60 Planner::setup();
64
65 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
66 throw Exception("The minimum valid path fraction must be in the range (0,1]");
67
68 dStart_.setDimension(projectionEvaluator_->getDimension());
69 dGoal_.setDimension(projectionEvaluator_->getDimension());
70}
71
73{
75 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
76
77 if (goal == nullptr)
78 {
79 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
81 }
82
84
85 while (const base::State *st = pis_.nextStart())
86 {
87 auto *motion = new Motion(si_);
88 si_->copyState(motion->state, st);
89 motion->root = st;
90 motion->valid = true;
91 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
92 dStart_.addMotion(motion, xcoord);
93 }
94
95 if (dStart_.getMotionCount() == 0)
96 {
97 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
99 }
100
101 if (!goal->couldSample())
102 {
103 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
105 }
106
107 if (!sampler_)
108 sampler_ = si_->allocStateSampler();
109
110 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
111 (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
112
113 base::State *xstate = si_->allocState();
114 bool startTree = true;
115 bool solved = false;
116
117 while (!ptc)
118 {
119 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
120 startTree = !startTree;
121 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
122 disc.countIteration();
123
124 // if we have not sampled too many goals already
125 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
126 {
127 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
128 if (st != nullptr)
129 {
130 auto *motion = new Motion(si_);
131 si_->copyState(motion->state, st);
132 motion->root = motion->state;
133 motion->valid = true;
134 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
135 dGoal_.addMotion(motion, xcoord);
136 }
137 if (dGoal_.getMotionCount() == 0)
138 {
139 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
140 break;
141 }
142 }
143
144 Discretization<Motion>::Cell *ecell = nullptr;
145 Motion *existing = nullptr;
146 disc.selectMotion(existing, ecell);
147 assert(existing);
148 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
149
150 /* create a motion */
151 auto *motion = new Motion(si_);
152 si_->copyState(motion->state, xstate);
153 motion->parent = existing;
154 motion->root = existing->root;
155 existing->children.push_back(motion);
156 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
157 disc.addMotion(motion, xcoord);
158
159 /* attempt to connect trees */
160 Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
161 if ((ocell != nullptr) && !ocell->data->motions.empty())
162 {
163 Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
164
165 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root,
166 startTree ? motion->root : connectOther->root))
167 {
168 auto *connect = new Motion(si_);
169 si_->copyState(connect->state, connectOther->state);
170 connect->parent = motion;
171 connect->root = motion->root;
172 motion->children.push_back(connect);
173 projectionEvaluator_->computeCoordinates(connect->state, xcoord);
174 disc.addMotion(connect, xcoord);
175
176 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
177 {
178 if (startTree)
179 connectionPoint_ = std::make_pair(connectOther->state, motion->state);
180 else
181 connectionPoint_ = std::make_pair(motion->state, connectOther->state);
182
183 /* extract the motions and put them in solution vector */
184
185 std::vector<Motion *> mpath1;
186 while (motion != nullptr)
187 {
188 mpath1.push_back(motion);
189 motion = motion->parent;
190 }
191
192 std::vector<Motion *> mpath2;
193 while (connectOther != nullptr)
194 {
195 mpath2.push_back(connectOther);
196 connectOther = connectOther->parent;
197 }
198
199 if (startTree)
200 mpath1.swap(mpath2);
201
202 auto path(std::make_shared<PathGeometric>(si_));
203 path->getStates().reserve(mpath1.size() + mpath2.size());
204 for (int i = mpath1.size() - 1; i >= 0; --i)
205 path->append(mpath1[i]->state);
206 for (auto &i : mpath2)
207 path->append(i->state);
208
209 pdef_->addSolutionPath(path, false, 0.0, getName());
210 solved = true;
211 break;
212 }
213 }
214 }
215 }
216
217 si_->freeState(xstate);
218
219 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on "
220 "boundary))",
221 getName().c_str(), dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(),
222 dGoal_.getMotionCount(), dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(),
223 dStart_.getGrid().countExternal(), dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
224
226}
227
229{
230 std::vector<Motion *> mpath;
231
232 /* construct the solution path */
233 while (motion != nullptr)
234 {
235 mpath.push_back(motion);
236 motion = motion->parent;
237 }
238
239 std::pair<base::State *, double> lastValid;
240 lastValid.first = temp;
241
242 /* check the path */
243 for (int i = mpath.size() - 1; i >= 0; --i)
244 if (!mpath[i]->valid)
245 {
246 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
247 mpath[i]->valid = true;
248 else
249 {
250 Motion *parent = mpath[i]->parent;
251 removeMotion(disc, mpath[i]);
252
253 // add the valid part of the path, if sufficiently long
254 if (lastValid.second > minValidPathFraction_)
255 {
256 auto *reAdd = new Motion(si_);
257 si_->copyState(reAdd->state, lastValid.first);
258 reAdd->parent = parent;
259 reAdd->root = parent->root;
260 parent->children.push_back(reAdd);
261 reAdd->valid = true;
263 projectionEvaluator_->computeCoordinates(reAdd->state, coord);
264 disc.addMotion(reAdd, coord);
265 }
266
267 return false;
268 }
269 }
270 return true;
271}
272
274{
275 /* remove from grid */
276
278 projectionEvaluator_->computeCoordinates(motion->state, coord);
279 disc.removeMotion(motion, coord);
280
281 /* remove self from parent list */
282
283 if (motion->parent != nullptr)
284 {
285 for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
286 if (motion->parent->children[i] == motion)
287 {
288 motion->parent->children.erase(motion->parent->children.begin() + i);
289 break;
290 }
291 }
292
293 /* remove children */
294 for (auto &i : motion->children)
295 {
296 i->parent = nullptr;
297 removeMotion(disc, i);
298 }
299
300 freeMotion(motion);
301}
302
304{
305 if (motion->state != nullptr)
306 si_->freeState(motion->state);
307 delete motion;
308}
309
311{
312 Planner::clear();
313
314 sampler_.reset();
315 dStart_.clear();
316 dGoal_.clear();
317 connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
318}
319
321{
322 Planner::getPlannerData(data);
323 dStart_.getPlannerData(data, 1, true, nullptr);
324 dGoal_.getPlannerData(data, 2, false, nullptr);
325
326 // Insert the edge connecting the two trees
327 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
328}
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
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...
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
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...
void freeMotion(Motion *motion)
Free the memory for a motion.
base::StateSamplerPtr sampler_
The employed state sampler.
Definition LBKPIECE1.h:209
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
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.