Loading...
Searching...
No Matches
ImplicitGraph.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019-present 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
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::setUseKNearest(bool useKNearest)
96 {
97 useKNearest_ = useKNearest;
98 }
99
101 {
102 return useKNearest_;
103 }
104
106 {
107 // Create a vertex corresponding to this state.
108 auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
109
110 // Copy the state into the vertex's state.
111 spaceInformation_->copyState(startVertex->getState(), startState);
112
113 // By definition, this has identity cost-to-come.
114 startVertex->setCostToComeFromStart(objective_->identityCost());
115
116 // Add the start vertex to the set of vertices.
117 vertices_.add(startVertex);
118
119 // Remember it as a start vertex.
120 startVertices_.emplace_back(startVertex);
121 }
122
124 {
125 // Create a vertex corresponding to this state.
126 auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
127
128 // Copy the state into the vertex's state.
129 spaceInformation_->copyState(goalVertex->getState(), goalState);
130
131 // Add the goal vertex to the set of vertices.
132 vertices_.add(goalVertex);
133
134 // Remember it as a goal vertex.
135 goalVertices_.emplace_back(goalVertex);
136 }
137
139 {
140 return !startVertices_.empty();
141 }
142
144 {
145 return !goalVertices_.empty();
146 }
147
148 void
151 {
152 // We need to keep track whether a new goal and/or a new start has been added.
153 bool addedNewGoalState = false;
154 bool addedNewStartState = false;
155
156 // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
157 // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
158 // wants us to wait for a goal.
159 do
160 {
161 // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
162 auto newGoalState = inputStates->nextGoal(terminationCondition);
163
164 // If there was a new valid goal, register it as such and remember that a goal has been added.
165 if (static_cast<bool>(newGoalState))
166 {
167 registerGoalState(newGoalState);
168 addedNewGoalState = true;
169 }
170
171 } while (inputStates->haveMoreGoalStates());
172
173 // Having updated the goals, we now update the starts.
174 while (inputStates->haveMoreStartStates())
175 {
176 // Get the next start. The returned pointer can be a nullptr (if the state is invalid).
177 auto newStartState = inputStates->nextStart();
178
179 // If there is a new valid start, register it as such and remember that a start has been added.
180 if (static_cast<bool>(newStartState))
181 {
182 registerStartState(newStartState);
183 addedNewStartState = true;
184 }
185 }
186
187 // If we added a new start and have previously pruned goals, we might want to add the goals back.
188 if (addedNewStartState && !prunedGoalVertices_.empty())
189 {
190 // Keep track of the pruned goal vertices that have been revived.
191 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
192
193 // Let's see if the pruned goal is close enough to any new start to revive it..
194 for (auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
195 {
196 // Loop over all start states to get the best cost.
197 auto heuristicCost = objective_->infiniteCost();
198 for (const auto &start : startVertices_)
199 {
200 heuristicCost = objective_->betterCost(
201 heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
202 }
203
204 // If this goal can possibly improve the current solution, add it back to the graph.
205 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
206 {
207 registerGoalState((*it)->getState());
208 addedNewGoalState = true;
209 revivedGoals.emplace_back(it);
210 }
211 }
212
213 // Remove all revived goals from the pruned goals.
214 for (const auto &revivedGoal : revivedGoals)
215 {
216 std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
217 prunedGoalVertices_.pop_back();
218 }
219 }
220
221 // If we added a new goal and have previously pruned starts, we might want to add the starts back.
222 if (addedNewGoalState && !prunedStartVertices_.empty())
223 {
224 // Keep track of the pruned goal vertices that have been revived.
225 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
226
227 // Let's see if the pruned start is close enough to any new goal to revive it..
228 for (auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
229 {
230 // Loop over all start states to get the best cost.
231 auto heuristicCost = objective_->infiniteCost();
232 for (const auto &goal : goalVertices_)
233 {
234 heuristicCost = objective_->betterCost(
235 heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
236 }
237
238 // If this goal can possibly improve the current solution, add it back to the graph.
239 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
240 {
241 registerStartState((*it)->getState());
242 addedNewStartState = true;
243 revivedStarts.emplace_back(it);
244 }
245 }
246
247 // Remove all revived goals from the pruned goals.
248 for (const auto &revivedStart : revivedStarts)
249 {
250 std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
251 prunedStartVertices_.pop_back();
252 }
253 }
254
255 if (addedNewGoalState || addedNewStartState)
256 {
257 // Allocate a state sampler if we have at least one start and one goal.
258 if (!startVertices_.empty() && !goalVertices_.empty())
259 {
260 sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
261 std::numeric_limits<unsigned int>::max());
262 }
263 }
264
265 if (!goalVertices_.empty() && startVertices_.empty())
266 {
267 OMPL_WARN("AIT* (ImplicitGraph): The problem has a goal but not a start. AIT* can not find a "
268 "solution since PlannerInputStates provides no method to wait for a valid start state to "
269 "appear.");
270 }
271 }
272
273 std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet() const
274 {
275 std::size_t numberOfSamplesInInformedSet{0u};
276 std::vector<std::shared_ptr<Vertex>> vertices;
277 vertices_.list(vertices);
278
279 // Loop over all vertices.
280 for (const auto &vertex : vertices)
281 {
282 // Get the best cost to come from any start.
283 ompl::base::Cost bestCostToComeHeuristic = objective_->infiniteCost();
284 for (const auto &start : startVertices_)
285 {
286 auto costToComeHeuristic =
287 objective_->motionCostHeuristic(start->getState(), vertex->getState());
288 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToComeHeuristic))
289 {
290 bestCostToComeHeuristic = costToComeHeuristic;
291 }
292 }
293
294 // Get the best cost to go to any goal.
295 ompl::base::Cost bestCostToGoHeuristic = objective_->infiniteCost();
296 for (const auto &goal : goalVertices_)
297 {
298 auto costToComeHeuristic =
299 objective_->motionCostHeuristic(vertex->getState(), goal->getState());
300 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToGoHeuristic))
301 {
302 bestCostToGoHeuristic = costToComeHeuristic;
303 }
304 }
305
306 // If this can possibly improve the current solution, it is in the informed set.
307 if (objective_->isCostBetterThan(
308 objective_->combineCosts(bestCostToComeHeuristic, bestCostToGoHeuristic), solutionCost_))
309 {
310 ++numberOfSamplesInInformedSet;
311 }
312 }
313
314 return numberOfSamplesInInformedSet;
315 }
316
317 std::vector<std::shared_ptr<Vertex>> ImplicitGraph::addSamples(std::size_t numNewSamples)
318 {
319 // First get the number of samples inside the informed set.
320 auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
321
322 // Create new vertices.
323 std::vector<std::shared_ptr<Vertex>> newVertices;
324 newVertices.reserve(numNewSamples);
325 while (newVertices.size() < numNewSamples)
326 {
327 // Create a new vertex.
328 newVertices.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
329
330 do
331 {
332 // Sample the associated state uniformly within the informed set.
333 sampler_->sampleUniform(newVertices.back()->getState(), solutionCost_);
334
335 // Count how many states we've checked.
336 ++numSampledStates_;
337 } while (!spaceInformation_->getStateValidityChecker()->isValid(newVertices.back()->getState()));
338
339 ++numValidSamples_;
340 }
341
342 // Add all new vertices to the nearest neighbor structure.
343 vertices_.add(newVertices);
344
345 auto numUniformSamplesInInformedSet =
346 numSamplesInInformedSet + numNewSamples - startVertices_.size() - goalVertices_.size();
347
348 // We need to do some internal housekeeping.
349 ++batchId_;
350 if (useKNearest_)
351 {
352 numNeighbors_ = computeNumberOfNeighbors(numUniformSamplesInInformedSet);
353 }
354 else
355 {
356 radius_ = computeConnectionRadius(numUniformSamplesInInformedSet);
357 }
358
359 return newVertices;
360 }
361
363 {
364 return vertices_.size();
365 }
366
368 {
369 return radius_;
370 }
371
372 std::vector<std::shared_ptr<Vertex>>
373 ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
374 {
375 // Return cached neighbors if available.
376 if (vertex->hasCachedNeighbors())
377 {
378 return vertex->getNeighbors();
379 }
380 else
381 {
382 ++numNearestNeighborsCalls_;
383 std::vector<std::shared_ptr<Vertex>> neighbors{};
384 if (useKNearest_)
385 {
386 vertices_.nearestK(vertex, numNeighbors_, neighbors);
387 }
388 else
389 {
390 vertices_.nearestR(vertex, radius_, neighbors);
391 }
392 vertex->cacheNeighbors(neighbors);
393 return neighbors;
394 }
395 }
396
397 bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
398 {
399 for (const auto &start : startVertices_)
400 {
401 if (vertex->getId() == start->getId())
402 {
403 return true;
404 }
405 }
406 return false;
407 }
408
409 bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
410 {
411 for (const auto &goal : goalVertices_)
412 {
413 if (vertex->getId() == goal->getId())
414 {
415 return true;
416 }
417 }
418 return false;
419 }
420
421 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
422 {
423 return startVertices_;
424 }
425
426 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
427 {
428 return goalVertices_;
429 }
430
431 std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
432 {
433 std::vector<std::shared_ptr<Vertex>> vertices;
434 vertices_.list(vertices);
435 return vertices;
436 }
437
439 {
440 if (!objective_->isFinite(solutionCost_))
441 {
442 return;
443 }
444
445 std::vector<std::shared_ptr<Vertex>> vertices;
446 vertices_.list(vertices);
447
448 // Prepare the vector of vertices to be pruned.
449 std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
450
451 // Check each vertex whether it can be pruned.
452 for (const auto &vertex : vertices)
453 {
454 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
455 // that is more expensive than the current solution.
456 if (!canPossiblyImproveSolution(vertex))
457 {
458 // We keep track of pruned start and goal vertices. This is because if the user adds start or
459 // goal states after we have pruned start or goal states, we might want to reconsider pruned
460 // start or goal states.
461 if (isGoal(vertex))
462 {
463 prunedGoalVertices_.emplace_back(vertex);
464 }
465 else if (isStart(vertex))
466 {
467 prunedStartVertices_.emplace_back(vertex);
468 }
469 verticesToBePruned.emplace_back(vertex);
470 }
471 }
472
473 // Remove all vertices to be pruned.
474 for (const auto &vertex : verticesToBePruned)
475 {
476 // Remove it from both search trees.
477 if (vertex->hasReverseParent())
478 {
479 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
480 vertex->resetReverseParent();
481 }
482 vertex->invalidateReverseBranch();
483 if (vertex->hasForwardParent())
484 {
485 vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
486 vertex->resetForwardParent();
487 }
488 vertex->invalidateForwardBranch();
489
490 // Remove it from the nearest neighbor struct.
491 vertices_.remove(vertex);
492 }
493
494 // Assert that the forward and reverse queue are empty?
495 }
496
498 {
499 return numSampledStates_;
500 }
501
503 {
504 return numValidSamples_;
505 }
506
508 {
509 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
510 // collision checked edges don't count here.)
511 return numSampledStates_;
512 }
513
515 {
516 return numNearestNeighborsCalls_;
517 }
518
519 double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
520 {
521 // Define the dimension as a helper variable.
522 auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
523
524 // Compute the RRT* factor.
525 return rewireFactor_ *
526 std::pow(2.0 * (1.0 + 1.0 / dimension) *
527 (sampler_->getInformedMeasure(solutionCost_) /
528 unitNBallMeasure(spaceInformation_->getStateDimension())) *
529 (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
530 1.0 / dimension);
531
532 // // Compute the FMT* factor.
533 // return 2.0 * rewireFactor_ *
534 // std::pow((1.0 / dimension) *
535 // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
536 // unitNBallMeasure(spaceInformation_->getStateDimension())) *
537 // (std::log(static_cast<double>(numSamples)) / numSamples),
538 // 1.0 / dimension);
539 }
540
541 std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
542 {
543 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
544 }
545
546 bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
547 {
548 // Get the preferred start for this vertex.
549 auto bestCostToCome = objective_->infiniteCost();
550 for (const auto &start : startVertices_)
551 {
552 auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
553 if (objective_->isCostBetterThan(costToCome, bestCostToCome))
554 {
555 bestCostToCome = costToCome;
556 }
557 }
558
559 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
560 // that is more expensive than the current solution.
561 return objective_->isCostBetterThan(
562 objective_->combineCosts(
563 bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
564 solutionCost_);
565 }
566
567 } // namespace aitstar
568
569 } // namespace geometric
570
571} // 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:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:237
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:355
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:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
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.
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.
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
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.
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.
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.
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.