Loading...
Searching...
No Matches
MultiQuotient.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#ifndef OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_MULTIQUOTIENT_
39#define OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_MULTIQUOTIENT_
40#include <ompl/geometric/planners/quotientspace/datastructures/QuotientSpace.h>
41#include <type_traits>
42#include <queue>
43
44namespace ompl
45{
46 namespace geometric
47 {
57 template <class T>
59 {
61 static_assert(std::is_base_of<QuotientSpace, T>::value, "Template must inherit from Quotient");
62
63 public:
64 const bool DEBUG{false};
65
68 MultiQuotient(std::vector<ompl::base::SpaceInformationPtr> &siVec, std::string type = "QuotientPlanner");
70 MultiQuotient(ompl::base::SpaceInformationPtr si, std::string type) = delete;
71
72 virtual ~MultiQuotient() override;
73
75 void getPlannerData(ompl::base::PlannerData &data) const override;
76
78 void setup() override;
79 void clear() override;
80 virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override;
81 const ompl::base::ProblemDefinitionPtr &getProblemDefinition(unsigned int kQuotientSpace) const;
82
84 int getLevels() const;
85
87 std::vector<int> getFeasibleNodes() const;
89 std::vector<int> getNodes() const;
90
92 std::vector<int> getDimensionsPerLevel() const;
93 void setStopLevel(unsigned int level_);
94
95 protected:
97 std::vector<ompl::base::PathPtr> solutions_;
98
100 std::vector<QuotientSpace *> quotientSpaces_;
101
104
106 unsigned int currentQuotientLevel_{0};
107
111 unsigned int stopAtLevel_;
112
114 std::vector<ompl::base::SpaceInformationPtr> siVec_;
115
118 {
119 // ">" operator: smallest value is top in queue
120 // "<" operator: largest value is top in queue (default)
121 bool operator()(const QuotientSpace *lhs, const QuotientSpace *rhs) const
122 {
123 return lhs->getImportance() < rhs->getImportance();
124 }
125 };
128 typedef std::priority_queue<QuotientSpace *, std::vector<QuotientSpace *>, CmpQuotientSpacePtrs>
130 QuotientSpacePriorityQueue priorityQueue_;
131 };
132 } // namespace geometric
133} // namespace ompl
134#include <ompl/geometric/planners/quotientspace/algorithms/MultiQuotientImpl.h>
135#endif
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:223
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition Planner.cpp:71
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
std::vector< int > getDimensionsPerLevel() const
Get all dimensions of the quotient-spaces in the sequence.
std::vector< int > getFeasibleNodes() const
Number of feasible nodes on each QuotientSpace (for DEBUGGING)
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
unsigned int currentQuotientLevel_
Current level on which we have not yet found a path.
std::vector< ompl::base::SpaceInformationPtr > siVec_
Each QuotientSpace has a unique ompl::base::SpaceInformationPtr.
std::vector< QuotientSpace * > quotientSpaces_
Sequence of quotient-spaces.
unsigned int stopAtLevel_
Sometimes we only want to plan until a certain quotient-space level (for debugging for example)....
bool foundKLevelSolution_
Indicator if a solution has been found on the current quotient-spaces.
int getLevels() const
Number of quotient-spaces.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::vector< int > getNodes() const
Number of nodes on each QuotientSpace (for DEBUGGING)
virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()....
void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about QuotientSpace level)
std::vector< ompl::base::PathPtr > solutions_
Solution paths on each quotient-space.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
std::priority_queue< QuotientSpace *, std::vector< QuotientSpace * >, CmpQuotientSpacePtrs > QuotientSpacePriorityQueue
Priority queue of QuotientSpaces which keeps track of how often every tree on each space has been exp...
A single quotient-space.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()