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>
58  class MultiQuotient : public ompl::base::Planner
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 
117  struct CmpQuotientSpacePtrs
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
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...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Base class for a planner.
Definition: Planner.h:287
std::vector< ompl::base::PathPtr > solutions_
Solution paths on each quotient-space.
A shared pointer wrapper for ompl::base::SpaceInformation.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
std::vector< int > getNodes() const
Number of nodes on each QuotientSpace (for DEBUGGING)
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...
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()....
std::vector< int > getFeasibleNodes() const
Number of feasible nodes on each QuotientSpace (for DEBUGGING)
int getLevels() const
Number of quotient-spaces.
unsigned int stopAtLevel_
Sometimes we only want to plan until a certain quotient-space level (for debugging for example)....
std::vector< QuotientSpace * > quotientSpaces_
Sequence of quotient-spaces.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int currentQuotientLevel_
Current level on which we have not yet found a path.
std::vector< int > getDimensionsPerLevel() const
Get all dimensions of the quotient-spaces in the sequence.
A class to store the exit status of Planner::solve()
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.
std::vector< ompl::base::SpaceInformationPtr > siVec_
Each QuotientSpace has a unique ompl::base::SpaceInformationPtr.
bool foundKLevelSolution_
Indicator if a solution has been found on the current quotient-spaces.
void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about QuotientSpace level)
Main namespace. Contains everything in this library.
Definition: AppBase.h:22