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();
61 tools::SelfConfig sc(si_, getName());
62 sc.configureProjectionEvaluator(projectionEvaluator_);
63 sc.configurePlannerRange(maxDistance_);
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{
74 checkValidity();
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
83 Discretization<Motion>::Coord xcoord(projectionEvaluator_->getDimension());
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;
262 Discretization<Motion>::Coord coord(projectionEvaluator_->getDimension());
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
277 Discretization<Motion>::Coord coord(projectionEvaluator_->getDimension());
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,...
Definition: PlannerData.h:175
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...
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
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBKPIECE1.cpp:310
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: LBKPIECE1.h:152
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
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...
Definition: LBKPIECE1.cpp:228
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: LBKPIECE1.cpp:303
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
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.
Definition: SelfConfig.cpp:225
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
#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
@ 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()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62