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 
44 namespace ompl
45 {
46  namespace geometric
47  {
57  template <class T>
59  {
60  using BaseT = ompl::base::Planner;
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 
103  bool foundKLevelSolution_{false};
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,...
Definition: PlannerData.h:175
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...
Definition: MultiQuotient.h:59
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)
MultiQuotient(std::vector< ompl::base::SpaceInformationPtr > &siVec, std::string type="QuotientPlanner")
Constructor taking a sequence of ompl::base::SpaceInformationPtr and computing the quotient-spaces fo...
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.
Definition: MultiQuotient.h:97
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.
Definition: QuotientSpace.h:49
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Compare function for priority queue.