Loading...
Searching...
No Matches
ImplicitGraph.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Oxford
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 names of the copyright holders 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// Authors: Marlin Strub
36#include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
37
38#include <cmath>
39
40#include <boost/math/constants/constants.hpp>
41
42#include "ompl/util/GeometricEquations.h"
43
44namespace ompl
45{
46 namespace geometric
47 {
48 namespace aitstar
49 {
51 : batchId_(1u), solutionCost_(solutionCost)
52 {
53 }
54
55 void ImplicitGraph::setup(const ompl::base::SpaceInformationPtr &spaceInformation,
56 const ompl::base::ProblemDefinitionPtr &problemDefinition,
58 {
59 vertices_.setDistanceFunction(
60 [this](const std::shared_ptr<Vertex> &a, const std::shared_ptr<Vertex> &b) {
61 return spaceInformation_->distance(a->getState(), b->getState());
62 });
63 spaceInformation_ = spaceInformation;
64 problemDefinition_ = problemDefinition;
65 objective_ = problemDefinition->getOptimizationObjective();
66 k_rgg_ = boost::math::constants::e<double>() +
67 (boost::math::constants::e<double>() / spaceInformation->getStateDimension());
69 }
70
72 {
73 batchId_ = 1u;
74 radius_ = std::numeric_limits<double>::infinity();
75 numNeighbors_ = std::numeric_limits<std::size_t>::max();
76 vertices_.clear();
77 startVertices_.clear();
78 goalVertices_.clear();
79 prunedStartVertices_.clear();
80 prunedGoalVertices_.clear();
81 numSampledStates_ = 0u;
82 numValidSamples_ = 0u;
83 }
84
85 void ImplicitGraph::setRewireFactor(double rewireFactor)
86 {
87 rewireFactor_ = rewireFactor;
88 }
89
91 {
92 return rewireFactor_;
93 }
94
95 void ImplicitGraph::setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
96 {
97 maxNumGoals_ = maxNumberOfGoals;
98 }
99
101 {
102 return maxNumGoals_;
103 }
104
105 void ImplicitGraph::setUseKNearest(bool useKNearest)
106 {
107 useKNearest_ = useKNearest;
108 }
109
111 {
112 return useKNearest_;
113 }
114
116 {
117 // Create a vertex corresponding to this state.
118 auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
119
120 // Copy the state into the vertex's state.
121 spaceInformation_->copyState(startVertex->getState(), startState);
122
123 // By definition, this has identity cost-to-come.
124 startVertex->setCostToComeFromStart(objective_->identityCost());
125
126 // Add the start vertex to the set of vertices.
127 vertices_.add(startVertex);
128
129 // Remember it as a start vertex.
130 startVertices_.emplace_back(startVertex);
131 }
132
134 {
135 // Create a vertex corresponding to this state.
136 auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
137
138 // Copy the state into the vertex's state.
139 spaceInformation_->copyState(goalVertex->getState(), goalState);
140
141 // Add the goal vertex to the set of vertices.
142 vertices_.add(goalVertex);
143
144 // Remember it as a goal vertex.
145 goalVertices_.emplace_back(goalVertex);
146 }
147
149 {
150 return !startVertices_.empty();
151 }
152
154 {
155 return !goalVertices_.empty();
156 }
157
158 void
161 {
162 // We need to keep track whether a new goal and/or a new start has been added.
163 bool addedNewGoalState = false;
164 bool addedNewStartState = false;
165
166 // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
167 // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
168 // wants us to wait for a goal.
169 do
170 {
171 // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
172 auto newGoalState = inputStates->nextGoal(terminationCondition);
173
174 // If there was a new valid goal, register it as such and remember that a goal has been added.
175 if (static_cast<bool>(newGoalState))
176 {
177 registerGoalState(newGoalState);
178 addedNewGoalState = true;
179 }
180
181 } while (inputStates->haveMoreGoalStates() && goalVertices_.size() <= maxNumGoals_);
182
183 // Having updated the goals, we now update the starts.
184 while (inputStates->haveMoreStartStates())
185 {
186 // Get the next start. The returned pointer can be a nullptr (if the state is invalid).
187 auto newStartState = inputStates->nextStart();
188
189 // If there is a new valid start, register it as such and remember that a start has been added.
190 if (static_cast<bool>(newStartState))
191 {
192 registerStartState(newStartState);
193 addedNewStartState = true;
194 }
195 }
196
197 // If we added a new start and have previously pruned goals, we might want to add the goals back.
198 if (addedNewStartState && !prunedGoalVertices_.empty())
199 {
200 // Keep track of the pruned goal vertices that have been revived.
201 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
202
203 // Let's see if the pruned goal is close enough to any new start to revive it..
204 for (auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
205 {
206 // Loop over all start states to get the best cost.
207 auto heuristicCost = objective_->infiniteCost();
208 for (const auto &start : startVertices_)
209 {
210 heuristicCost = objective_->betterCost(
211 heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
212 }
213
214 // If this goal can possibly improve the current solution, add it back to the graph.
215 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
216 {
217 registerGoalState((*it)->getState());
218 addedNewGoalState = true;
219 revivedGoals.emplace_back(it);
220 }
221 }
222
223 // Remove all revived goals from the pruned goals.
224 for (const auto &revivedGoal : revivedGoals)
225 {
226 std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
227 prunedGoalVertices_.pop_back();
228 }
229 }
230
231 // If we added a new goal and have previously pruned starts, we might want to add the starts back.
232 if (addedNewGoalState && !prunedStartVertices_.empty())
233 {
234 // Keep track of the pruned goal vertices that have been revived.
235 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
236
237 // Let's see if the pruned start is close enough to any new goal to revive it..
238 for (auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
239 {
240 // Loop over all start states to get the best cost.
241 auto heuristicCost = objective_->infiniteCost();
242 for (const auto &goal : goalVertices_)
243 {
244 heuristicCost = objective_->betterCost(
245 heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
246 }
247
248 // If this goal can possibly improve the current solution, add it back to the graph.
249 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
250 {
251 registerStartState((*it)->getState());
252 addedNewStartState = true;
253 revivedStarts.emplace_back(it);
254 }
255 }
256
257 // Remove all revived goals from the pruned goals.
258 for (const auto &revivedStart : revivedStarts)
259 {
260 std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
261 prunedStartVertices_.pop_back();
262 }
263 }
264
265 if (addedNewGoalState || addedNewStartState)
266 {
267 // Allocate a state sampler if we have at least one start and one goal.
268 if (!startVertices_.empty() && !goalVertices_.empty())
269 {
270 sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
271 std::numeric_limits<unsigned int>::max());
272 }
273 }
274
275 if (!goalVertices_.empty() && startVertices_.empty())
276 {
277 OMPL_WARN("AIT* (ImplicitGraph): The problem has a goal but not a start. AIT* can not find a "
278 "solution since PlannerInputStates provides no method to wait for a valid start state to "
279 "appear.");
280 }
281 }
282
283 std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet() const
284 {
285 // Loop over all vertices and count the ones in the informed set.
286 std::size_t numberOfSamplesInInformedSet{0u};
287 for (const auto &vertex : getVertices())
288 {
289 // Get the best cost to come from any start.
290 auto costToCome = objective_->infiniteCost();
291 for (const auto &start : startVertices_)
292 {
293 costToCome = objective_->betterCost(
294 costToCome, objective_->motionCostHeuristic(start->getState(), vertex->getState()));
295 }
296
297 // Get the best cost to go to any goal.
298 auto costToGo = objective_->infiniteCost();
299 for (const auto &goal : goalVertices_)
300 {
301 costToGo = objective_->betterCost(
302 costToCome, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
303 }
304
305 // If this can possibly improve the current solution, it is in the informed set.
306 if (objective_->isCostBetterThan(objective_->combineCosts(costToCome, costToGo), solutionCost_))
307 {
308 ++numberOfSamplesInInformedSet;
309 }
310 }
311
312 return numberOfSamplesInInformedSet;
313 }
314
315 bool ImplicitGraph::addSamples(std::size_t numNewSamples,
316 const ompl::base::PlannerTerminationCondition &terminationCondition)
317 {
318 // If there are no states to be added, then there's nothing to do.
319 if (numNewSamples == 0u)
320 {
321 return true;
322 }
323
324 // Ensure there's enough space for the new samples.
325 newSamples_.reserve(numNewSamples);
326
327 do
328 {
329 // Create a new vertex.
330 newSamples_.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
331
332 do
333 {
334 // Sample the associated state uniformly within the informed set.
335 sampler_->sampleUniform(newSamples_.back()->getState(), solutionCost_);
336
337 // Count how many states we've checked.
338 ++numSampledStates_;
339 } while (!spaceInformation_->getStateValidityChecker()->isValid(newSamples_.back()->getState()));
340
341 // If this state happens to satisfy the goal condition, add it as such.
342 if (problemDefinition_->getGoal()->isSatisfied(newSamples_.back()->getState()))
343 {
344 goalVertices_.emplace_back(newSamples_.back());
345 newSamples_.back()->setCostToComeFromGoal(objective_->identityCost());
346 }
347
348 ++numValidSamples_;
349 } while (newSamples_.size() < numNewSamples && !terminationCondition);
350
351 if (newSamples_.size() == numNewSamples)
352 {
353 // First get the number of samples inside the informed set.
354 auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
355
356 if (useKNearest_)
357 {
358 numNeighbors_ = computeNumberOfNeighbors(numSamplesInInformedSet + numNewSamples -
359 startVertices_.size() - goalVertices_.size());
360 }
361 else
362 {
363 radius_ = computeConnectionRadius(numSamplesInInformedSet + numNewSamples -
364 startVertices_.size() - goalVertices_.size());
365 }
366
367 // Add all new vertices to the nearest neighbor structure.
368 vertices_.add(newSamples_);
369 newSamples_.clear();
370
371 // Update the batch id.
372 ++batchId_;
373
374 return true;
375 }
376
377 return false;
378 }
379
381 {
382 return vertices_.size();
383 }
384
386 {
387 return radius_;
388 }
389
390 std::vector<std::shared_ptr<Vertex>>
391 ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
392 {
393 // Return cached neighbors if available.
394 if (vertex->hasCachedNeighbors())
395 {
396 return vertex->getNeighbors();
397 }
398 else
399 {
400 ++numNearestNeighborsCalls_;
401 std::vector<std::shared_ptr<Vertex>> neighbors{};
402 if (useKNearest_)
403 {
404 vertices_.nearestK(vertex, numNeighbors_, neighbors);
405 }
406 else
407 {
408 vertices_.nearestR(vertex, radius_, neighbors);
409 }
410 vertex->cacheNeighbors(neighbors);
411 return neighbors;
412 }
413 }
414
415 bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
416 {
417 for (const auto &start : startVertices_)
418 {
419 if (vertex->getId() == start->getId())
420 {
421 return true;
422 }
423 }
424 return false;
425 }
426
427 bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
428 {
429 for (const auto &goal : goalVertices_)
430 {
431 if (vertex->getId() == goal->getId())
432 {
433 return true;
434 }
435 }
436 return false;
437 }
438
439 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
440 {
441 return startVertices_;
442 }
443
444 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
445 {
446 return goalVertices_;
447 }
448
449 std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
450 {
451 std::vector<std::shared_ptr<Vertex>> vertices;
452 vertices_.list(vertices);
453 return vertices;
454 }
455
457 {
458 if (!objective_->isFinite(solutionCost_))
459 {
460 return;
461 }
462
463 std::vector<std::shared_ptr<Vertex>> vertices;
464 vertices_.list(vertices);
465
466 // Prepare the vector of vertices to be pruned.
467 std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
468
469 // Check each vertex whether it can be pruned.
470 for (const auto &vertex : vertices)
471 {
472 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
473 // that is more expensive than the current solution.
474 if (!canPossiblyImproveSolution(vertex))
475 {
476 // We keep track of pruned start and goal vertices. This is because if the user adds start or
477 // goal states after we have pruned start or goal states, we might want to reconsider pruned
478 // start or goal states.
479 if (isGoal(vertex))
480 {
481 prunedGoalVertices_.emplace_back(vertex);
482 }
483 else if (isStart(vertex))
484 {
485 prunedStartVertices_.emplace_back(vertex);
486 }
487 verticesToBePruned.emplace_back(vertex);
488 }
489 }
490
491 // Remove all vertices to be pruned.
492 for (const auto &vertex : verticesToBePruned)
493 {
494 // Remove it from both search trees.
495 if (vertex->hasReverseParent())
496 {
497 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
498 vertex->resetReverseParent();
499 }
500 vertex->invalidateReverseBranch();
501 if (vertex->hasForwardParent())
502 {
503 vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
504 vertex->resetForwardParent();
505 }
506 vertex->invalidateForwardBranch();
507
508 // Remove it from the nearest neighbor struct.
509 vertices_.remove(vertex);
510 }
511
512 // Assert that the forward and reverse queue are empty?
513 }
514
516 {
517 return numSampledStates_;
518 }
519
521 {
522 return numValidSamples_;
523 }
524
526 {
527 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
528 // collision checked edges don't count here.)
529 return numSampledStates_;
530 }
531
533 {
534 return numNearestNeighborsCalls_;
535 }
536
537 double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
538 {
539 // Define the dimension as a helper variable.
540 auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
541
542 // Compute the RRT* factor.
543 return rewireFactor_ *
544 std::pow(2.0 * (1.0 + 1.0 / dimension) *
545 (sampler_->getInformedMeasure(solutionCost_) /
546 unitNBallMeasure(spaceInformation_->getStateDimension())) *
547 (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
548 1.0 / dimension);
549
550 // // Compute the FMT* factor.
551 // return 2.0 * rewireFactor_ *
552 // std::pow((1.0 / dimension) *
553 // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
554 // unitNBallMeasure(spaceInformation_->getStateDimension())) *
555 // (std::log(static_cast<double>(numSamples)) / numSamples),
556 // 1.0 / dimension);
557 }
558
559 std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
560 {
561 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
562 }
563
564 bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
565 {
566 // Get the preferred start for this vertex.
567 auto bestCostToCome = objective_->infiniteCost();
568 for (const auto &start : startVertices_)
569 {
570 auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
571 if (objective_->isCostBetterThan(costToCome, bestCostToCome))
572 {
573 bestCostToCome = costToCome;
574 }
575 }
576
577 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
578 // that is more expensive than the current solution.
579 return objective_->isCostBetterThan(
580 objective_->combineCosts(
581 bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
582 solutionCost_);
583 }
584
585 } // namespace aitstar
586
587 } // namespace geometric
588
589} // namespace ompl
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition Planner.h:78
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition Planner.cpp:339
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:228
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:346
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition Planner.cpp:265
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition State.h:50
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
bool hasAStartState() const
Returns whether the graph has a goal state.
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
double getRewireFactor() const
Get the reqire factor of the RGG.
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
void prune()
Prune all samples that can not contribute to a solution better than the current one.
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
void clear()
Resets the graph to its construction state, without resetting options.
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
double getConnectionRadius() const
Gets the RGG connection radius.
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
std::size_t getNumVertices() const
Gets the number of samples in the graph.
bool hasAGoalState() const
Returns whether the graph has a goal state.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.