Loading...
Searching...
No Matches
Syclop.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: Matt Maly */
36
37#include "ompl/control/planners/syclop/Syclop.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/base/ProblemDefinition.h"
40#include "ompl/util/DisableCompilerWarning.h"
41#include <limits>
42#include <stack>
43#include <algorithm>
44
45const double ompl::control::Syclop::Defaults::PROB_ABANDON_LEAD_EARLY = 0.25;
46const double ompl::control::Syclop::Defaults::PROB_KEEP_ADDING_TO_AVAIL = 0.50;
47const double ompl::control::Syclop::Defaults::PROB_SHORTEST_PATH = 0.95;
48
50{
52 if (!leadComputeFn)
53 setLeadComputeFn([this](int startRegion, int goalRegion, std::vector<int> &lead)
54 {
55 defaultComputeLead(startRegion, goalRegion, lead);
56 });
57 buildGraph();
58 addEdgeCostFactor([this](int r, int s)
59 {
60 return defaultEdgeCost(r, s);
61 });
62}
63
65{
67 lead_.clear();
68 availDist_.clear();
69 clearEdgeCostFactors();
70 clearGraphDetails();
71 startRegions_.clear();
72 goalRegions_.clear();
73}
74
76{
77 checkValidity();
78 if (!graphReady_)
79 {
80 numMotions_ = 0;
81 setupRegionEstimates();
82 setupEdgeEstimates();
83 graphReady_ = true;
84 }
85 while (const base::State *s = pis_.nextStart())
86 {
87 const int region = decomp_->locateRegion(s);
88 startRegions_.insert(region);
89 Motion *startMotion = addRoot(s);
90 graph_[boost::vertex(region, graph_)].motions.push_back(startMotion);
91 ++numMotions_;
92 updateCoverageEstimate(graph_[boost::vertex(region, graph_)], s);
93 }
94 if (startRegions_.empty())
95 {
96 OMPL_ERROR("%s: There are no valid start states", getName().c_str());
98 }
99
100 // We need at least one valid goal sample so that we can find the goal region
101 if (goalRegions_.empty())
102 {
103 if (const base::State *g = pis_.nextGoal(ptc))
104 goalRegions_.insert(decomp_->locateRegion(g));
105 else
106 {
107 OMPL_ERROR("%s: Unable to sample a valid goal state", getName().c_str());
109 }
110 }
111
112 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), numMotions_);
113
114 std::vector<Motion *> newMotions;
115 const Motion *solution = nullptr;
116 base::Goal *goal = pdef_->getGoal().get();
117 double goalDist = std::numeric_limits<double>::infinity();
118 bool solved = false;
119 while (!ptc && !solved)
120 {
121 const int chosenStartRegion = startRegions_.sampleUniform();
122 int chosenGoalRegion = -1;
123
124 // if we have not sampled too many goal regions already
125 if (pis_.haveMoreGoalStates() && goalRegions_.size() < numMotions_ / 2)
126 {
127 if (const base::State *g = pis_.nextGoal())
128 {
129 chosenGoalRegion = decomp_->locateRegion(g);
130 goalRegions_.insert(chosenGoalRegion);
131 }
132 }
133 if (chosenGoalRegion == -1)
134 chosenGoalRegion = goalRegions_.sampleUniform();
135
136 leadComputeFn(chosenStartRegion, chosenGoalRegion, lead_);
137 computeAvailableRegions();
138 for (int i = 0; i < numRegionExpansions_ && !solved && !ptc; ++i)
139 {
140 const int region = selectRegion();
141 bool improved = false;
142 for (int j = 0; j < numTreeSelections_ && !solved && !ptc; ++j)
143 {
144 newMotions.clear();
145 selectAndExtend(graph_[boost::vertex(region, graph_)], newMotions);
146 for (std::vector<Motion *>::const_iterator m = newMotions.begin(); m != newMotions.end() && !ptc; ++m)
147 {
148 Motion *motion = *m;
149 double distance;
150 solved = goal->isSatisfied(motion->state, &distance);
151 if (solved)
152 {
153 goalDist = distance;
154 solution = motion;
155 break;
156 }
157
158 // Check for approximate (best-so-far) solution
159 if (distance < goalDist)
160 {
161 goalDist = distance;
162 solution = motion;
163 }
164 const int newRegion = decomp_->locateRegion(motion->state);
165 graph_[boost::vertex(newRegion, graph_)].motions.push_back(motion);
166 ++numMotions_;
167 Region &newRegionObj = graph_[boost::vertex(newRegion, graph_)];
168 improved |= updateCoverageEstimate(newRegionObj, motion->state);
169 /* If tree has just crossed from one region to its neighbor,
170 update the connection estimates. If the tree has crossed an entire region,
171 then region and newRegion are not adjacent, and so we do not update estimates. */
172 if (newRegion != region && regionsToEdge_.count(std::pair<int, int>(region, newRegion)) > 0)
173 {
174 Adjacency *adj = regionsToEdge_[std::pair<int, int>(region, newRegion)];
175 adj->empty = false;
176 ++adj->numSelections;
177 improved |= updateConnectionEstimate(graph_[boost::vertex(region, graph_)], newRegionObj,
178 motion->state);
179 }
180
181 /* If this region already exists in availDist, update its weight. */
182 if (newRegionObj.pdfElem != nullptr)
183 availDist_.update(newRegionObj.pdfElem, newRegionObj.weight);
184 /* Otherwise, only add this region to availDist
185 if it already exists in the lead. */
186 else if (std::find(lead_.begin(), lead_.end(), newRegion) != lead_.end())
187 {
188 PDF<int>::Element *elem = availDist_.add(newRegion, newRegionObj.weight);
189 newRegionObj.pdfElem = elem;
190 }
191 }
192 }
193 if (!improved && rng_.uniform01() < probAbandonLeadEarly_)
194 break;
195 }
196 }
197 bool addedSolution = false;
198 if (solution != nullptr)
199 {
200 std::vector<const Motion *> mpath;
201 while (solution != nullptr)
202 {
203 mpath.push_back(solution);
204 solution = solution->parent;
205 }
206 auto path(std::make_shared<PathControl>(si_));
207 for (int i = mpath.size() - 1; i >= 0; --i)
208 if (mpath[i]->parent != nullptr)
209 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
210 else
211 path->append(mpath[i]->state);
212 pdef_->addSolutionPath(path, !solved, goalDist, getName());
213 addedSolution = true;
214 }
216}
217
219{
220 leadComputeFn = compute;
221}
222
224{
225 edgeCostFactors_.push_back(factor);
226}
227
229{
230 edgeCostFactors_.clear();
231}
232
233void ompl::control::Syclop::initRegion(Region &r)
234{
235 r.numSelections = 0;
236 r.volume = 1.0;
237 r.percentValidCells = 1.0;
238 r.freeVolume = 1.0;
239 r.pdfElem = nullptr;
240}
241
242void ompl::control::Syclop::setupRegionEstimates()
243{
244 std::vector<int> numTotal(decomp_->getNumRegions(), 0);
245 std::vector<int> numValid(decomp_->getNumRegions(), 0);
246 base::StateValidityCheckerPtr checker = si_->getStateValidityChecker();
247 base::StateSamplerPtr sampler = si_->allocStateSampler();
248 base::State *s = si_->allocState();
249
250 for (int i = 0; i < numFreeVolSamples_; ++i)
251 {
252 sampler->sampleUniform(s);
253 int rid = decomp_->locateRegion(s);
254 if (rid >= 0)
255 {
256 if (checker->isValid(s))
257 ++numValid[rid];
258 ++numTotal[rid];
259 }
260 }
261 si_->freeState(s);
262
263 for (int i = 0; i < decomp_->getNumRegions(); ++i)
264 {
265 Region &r = graph_[boost::vertex(i, graph_)];
266 r.volume = decomp_->getRegionVolume(i);
267 if (numTotal[i] == 0)
268 r.percentValidCells = 1.0;
269 else
270 r.percentValidCells = ((double)numValid[i]) / (double)numTotal[i];
271 r.freeVolume = r.percentValidCells * r.volume;
272 if (r.freeVolume < std::numeric_limits<double>::epsilon())
273 r.freeVolume = std::numeric_limits<double>::epsilon();
274 updateRegion(r);
275 }
276}
277
278void ompl::control::Syclop::updateRegion(Region &r)
279{
280 const double f = r.freeVolume * r.freeVolume * r.freeVolume * r.freeVolume;
281 r.alpha = 1.0 / ((1 + r.covGridCells.size()) * f);
282 r.weight = f / ((1 + r.covGridCells.size()) * (1 + r.numSelections * r.numSelections));
283}
284
285void ompl::control::Syclop::initEdge(Adjacency &adj, const Region *source, const Region *target)
286{
287 adj.source = source;
288 adj.target = target;
289 updateEdge(adj);
290 regionsToEdge_[std::pair<int, int>(source->index, target->index)] = &adj;
291}
292
293void ompl::control::Syclop::setupEdgeEstimates()
294{
295OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
296 EdgeIter ei, eend;
297OMPL_POP_GCC
298 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
299 {
300 Adjacency &adj = graph_[*ei];
301 adj.empty = true;
302 adj.numLeadInclusions = 0;
303 adj.numSelections = 0;
304 updateEdge(adj);
305 }
306}
307
308void ompl::control::Syclop::updateEdge(Adjacency &a)
309{
310 a.cost = 1.0;
311 for (const auto &factor : edgeCostFactors_)
312 {
313 a.cost *= factor(a.source->index, a.target->index);
314 }
315}
316
317bool ompl::control::Syclop::updateCoverageEstimate(Region &r, const base::State *s)
318{
319 const int covCell = covGrid_.locateRegion(s);
320 if (r.covGridCells.count(covCell) == 1)
321 return false;
322 r.covGridCells.insert(covCell);
323 updateRegion(r);
324 return true;
325}
326
327bool ompl::control::Syclop::updateConnectionEstimate(const Region &c, const Region &d, const base::State *s)
328{
329 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(c.index, d.index)];
330 const int covCell = covGrid_.locateRegion(s);
331 if (adj.covGridCells.count(covCell) == 1)
332 return false;
333 adj.covGridCells.insert(covCell);
334 updateEdge(adj);
335 return true;
336}
337
338void ompl::control::Syclop::buildGraph()
339{
340 VertexIndexMap index = get(boost::vertex_index, graph_);
341 std::vector<int> neighbors;
342 for (int i = 0; i < decomp_->getNumRegions(); ++i)
343 {
344 const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
345 Region &r = graph_[boost::vertex(v, graph_)];
346 initRegion(r);
347 r.index = index[v];
348 }
349 VertexIter vi, vend;
350 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
351 {
352 /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
353 and initialize the edge's Adjacency object. */
354 decomp_->getNeighbors(index[*vi], neighbors);
355 for (const auto &j : neighbors)
356 {
357 RegionGraph::edge_descriptor edge;
358 bool ignore;
359 boost::tie(edge, ignore) = boost::add_edge(*vi, boost::vertex(j, graph_), graph_);
360 initEdge(graph_[edge], &graph_[*vi], &graph_[boost::vertex(j, graph_)]);
361 }
362 neighbors.clear();
363 }
364}
365
366void ompl::control::Syclop::clearGraphDetails()
367{
368 VertexIter vi, vend;
369 for (boost::tie(vi, vend) = boost::vertices(graph_); vi != vend; ++vi)
370 graph_[*vi].clear();
371OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
372 EdgeIter ei, eend;
373OMPL_POP_GCC
374 for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
375 graph_[*ei].clear();
376 graphReady_ = false;
377}
378
379int ompl::control::Syclop::selectRegion()
380{
381 const int index = availDist_.sample(rng_.uniform01());
382 Region &region = graph_[boost::vertex(index, graph_)];
383 ++region.numSelections;
384 updateRegion(region);
385 return index;
386}
387
388void ompl::control::Syclop::computeAvailableRegions()
389{
390 for (unsigned int i = 0; i < availDist_.size(); ++i)
391 graph_[boost::vertex(availDist_[i], graph_)].pdfElem = nullptr;
392 availDist_.clear();
393 for (int i = lead_.size() - 1; i >= 0; --i)
394 {
395 Region &r = graph_[boost::vertex(lead_[i], graph_)];
396 if (!r.motions.empty())
397 {
398 r.pdfElem = availDist_.add(lead_[i], r.weight);
399 if (rng_.uniform01() >= probKeepAddingToAvail_)
400 break;
401 }
402 }
403}
404
405void ompl::control::Syclop::defaultComputeLead(int startRegion, int goalRegion, std::vector<int> &lead)
406{
407 lead.clear();
408 if (startRegion == goalRegion)
409 {
410 lead.push_back(startRegion);
411 return;
412 }
413
414 if (rng_.uniform01() < probShortestPath_)
415 {
416 std::vector<RegionGraph::vertex_descriptor> parents(decomp_->getNumRegions());
417 std::vector<double> distances(decomp_->getNumRegions());
418
419 try
420 {
421 boost::astar_search(graph_, boost::vertex(startRegion, graph_),
422 DecompositionHeuristic(this, getRegionFromIndex(goalRegion)),
423 boost::weight_map(get(&Adjacency::cost, graph_))
424 .distance_map(boost::make_iterator_property_map(distances.begin(),
425 get(boost::vertex_index, graph_)))
426 .predecessor_map(boost::make_iterator_property_map(
427 parents.begin(), get(boost::vertex_index, graph_)))
428 .visitor(GoalVisitor(goalRegion)));
429 }
430 catch (found_goal fg)
431 {
432 int region = goalRegion;
433 int leadLength = 1;
434
435 while (region != startRegion)
436 {
437 region = parents[region];
438 ++leadLength;
439 }
440 lead.resize(leadLength);
441 region = goalRegion;
442 for (int i = leadLength - 1; i >= 0; --i)
443 {
444 lead[i] = region;
445 region = parents[region];
446 }
447 }
448 }
449 else
450 {
451 /* Run a random-DFS over the decomposition graph from the start region to the goal region.
452 There may be a way to do this using boost::depth_first_search. */
453 VertexIndexMap index = get(boost::vertex_index, graph_);
454 std::stack<int> nodesToProcess;
455 std::vector<int> parents(decomp_->getNumRegions(), -1);
456 parents[startRegion] = startRegion;
457 nodesToProcess.push(startRegion);
458 bool goalFound = false;
459 while (!goalFound && !nodesToProcess.empty())
460 {
461 const int v = nodesToProcess.top();
462 nodesToProcess.pop();
463 std::vector<int> neighbors;
464 boost::graph_traits<RegionGraph>::adjacency_iterator ai, aend;
465 for (boost::tie(ai, aend) = adjacent_vertices(boost::vertex(v, graph_), graph_); ai != aend; ++ai)
466 {
467 if (parents[index[*ai]] < 0)
468 {
469 neighbors.push_back(index[*ai]);
470 parents[index[*ai]] = v;
471 }
472 }
473 for (std::size_t i = 0; i < neighbors.size(); ++i)
474 {
475 const int choice = rng_.uniformInt(i, neighbors.size() - 1);
476 if (neighbors[choice] == goalRegion)
477 {
478 int region = goalRegion;
479 int leadLength = 1;
480 while (region != startRegion)
481 {
482 region = parents[region];
483 ++leadLength;
484 }
485 lead.resize(leadLength);
486 region = goalRegion;
487 for (int j = leadLength - 1; j >= 0; --j)
488 {
489 lead[j] = region;
490 region = parents[region];
491 }
492 goalFound = true;
493 break;
494 }
495 nodesToProcess.push(neighbors[choice]);
496 std::swap(neighbors[i], neighbors[choice]);
497 }
498 }
499 }
500
501 // Now that we have a lead, update the edge weights.
502 for (std::size_t i = 0; i < lead.size() - 1; ++i)
503 {
504 Adjacency &adj = *regionsToEdge_[std::pair<int, int>(lead[i], lead[i + 1])];
505 if (adj.empty)
506 {
507 ++adj.numLeadInclusions;
508 updateEdge(adj);
509 }
510 }
511}
512
513double ompl::control::Syclop::defaultEdgeCost(int r, int s)
514{
515 const Adjacency &a = *regionsToEdge_[std::pair<int, int>(r, s)];
516 double factor = 1.0;
517 const int nsel = (a.empty ? a.numLeadInclusions : a.numSelections);
518 factor = (1.0 + (double)nsel * nsel) / (1.0 + (double)a.covGridCells.size() * a.covGridCells.size());
519 factor *= (a.source->alpha * a.target->alpha);
520 return factor;
521}
A class that will hold data contained in the PDF.
Definition PDF.h:53
Abstract definition of goals.
Definition Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition Planner.cpp:118
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Planner.cpp:92
Definition of an abstract state.
Definition State.h:50
Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to...
Definition Syclop.h:322
bool empty
This value is true if and only if this adjacency's source and target regions both contain zero tree m...
Definition Syclop.h:347
int numSelections
The number of times the low-level tree planner has selected motions from the source region when attem...
Definition Syclop.h:344
Representation of a motion.
Definition Syclop.h:257
const Motion * parent
The parent motion in the tree.
Definition Syclop.h:272
base::State * state
The state contained by the motion.
Definition Syclop.h:268
Representation of a region in the Decomposition assigned to Syclop.
Definition Syclop.h:279
PDF< int >::Element * pdfElem
The Element corresponding to this region in the PDF of available regions.
Definition Syclop.h:316
double weight
The probabilistic weight of this region, used when sampling from PDF.
Definition Syclop.h:308
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Syclop.cpp:49
std::function< double(int, int)> EdgeCostFactorFn
Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge co...
Definition Syclop.h:96
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition Syclop.cpp:64
void clearEdgeCostFactors()
Clears all edge cost factors, making all edge weights equivalent to 1.
Definition Syclop.cpp:228
void addEdgeCostFactor(const EdgeCostFactorFn &factor)
Adds an edge cost factor to be used for edge weights between adjacent regions.
Definition Syclop.cpp:223
std::function< void(int, int, std::vector< int > &)> LeadComputeFn
Leads should consist of a path of adjacent regions in the decomposition that start with the start reg...
Definition Syclop.h:100
void setLeadComputeFn(const LeadComputeFn &compute)
Allows the user to override the lead computation function.
Definition Syclop.cpp:218
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met....
Definition Syclop.cpp:75
#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
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.
@ TIMEOUT
The planner failed to find a solution.