MultiQuotientImpl.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 #include <ompl/geometric/planners/quotientspace/datastructures/PlannerDataVertexAnnotated.h>
38 #include <ompl/base/spaces/SO2StateSpace.h>
39 #include <ompl/base/spaces/SO3StateSpace.h>
40 #include <ompl/base/goals/GoalSampleableRegion.h>
41 #include <ompl/base/goals/GoalState.h>
42 #include <ompl/util/Exception.h>
43 #include <ompl/util/Time.h>
44 #include <queue>
45 
46 template <class T>
47 ompl::geometric::MultiQuotient<T>::MultiQuotient(std::vector<ompl::base::SpaceInformationPtr> &siVec, std::string type)
48  : ompl::base::Planner(siVec.back(), type), siVec_(siVec)
49 {
50  T::resetCounter();
51  for (unsigned int k = 0; k < siVec_.size(); k++)
52  {
53  QuotientSpace *parent = nullptr;
54  if (k > 0)
55  parent = quotientSpaces_.back();
56 
57  T *ss = new T(siVec_.at(k), parent);
58  quotientSpaces_.push_back(ss);
59  quotientSpaces_.back()->setLevel(k);
60  }
62  OMPL_DEVMSG2("Created %d QuotientSpace levels.", siVec_.size());
63 }
64 
65 template <class T>
67 {
68  return stopAtLevel_;
69 }
70 
71 template <class T>
73 {
74  std::vector<int> nodesPerLevel;
75  for (unsigned int k = 0; k < stopAtLevel_; k++)
76  {
77  unsigned int Nk = quotientSpaces_.at(k)->getTotalNumberOfSamples();
78  nodesPerLevel.push_back(Nk);
79  }
80  return nodesPerLevel;
81 }
82 
83 template <class T>
85 {
86  std::vector<int> feasibleNodesPerLevel;
87  for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
88  {
89  unsigned int Nk = quotientSpaces_.at(k)->getTotalNumberOfFeasibleSamples();
90  feasibleNodesPerLevel.push_back(Nk);
91  }
92  return feasibleNodesPerLevel;
93 }
94 
95 template <class T>
97 {
98  std::vector<int> dimensionsPerLevel;
99  for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
100  {
101  unsigned int Nk = quotientSpaces_.at(k)->getDimension();
102  dimensionsPerLevel.push_back(Nk);
103  }
104  return dimensionsPerLevel;
105 }
106 
107 template <class T>
109 {
110 }
111 
112 template <class T>
114 {
115  BaseT::setup();
116  for (unsigned int k = 0; k < stopAtLevel_; k++)
117  {
118  quotientSpaces_.at(k)->setup();
119  }
120  currentQuotientLevel_ = 0;
121 }
122 
123 template <class T>
124 void ompl::geometric::MultiQuotient<T>::setStopLevel(unsigned int level_)
125 {
126  if (level_ > quotientSpaces_.size())
127  {
128  stopAtLevel_ = quotientSpaces_.size();
129  }
130  else
131  {
132  stopAtLevel_ = level_;
133  }
134 }
135 
136 template <class T>
138 {
139  Planner::clear();
140 
141  for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
142  {
143  quotientSpaces_.at(k)->clear();
144  }
145  currentQuotientLevel_ = 0;
146 
147  while (!priorityQueue_.empty())
148  priorityQueue_.pop();
149  foundKLevelSolution_ = false;
150 
151  solutions_.clear();
152  pdef_->clearSolutionPaths();
153 }
154 
155 template <class T>
157 {
159 
160  for (unsigned int k = currentQuotientLevel_; k < stopAtLevel_; k++)
161  {
162  foundKLevelSolution_ = false;
163 
164  if (priorityQueue_.size() <= currentQuotientLevel_)
165  priorityQueue_.push(quotientSpaces_.at(k));
166 
167  ompl::base::PlannerTerminationCondition ptcOrSolutionFound(
168  [this, &ptc] { return ptc || foundKLevelSolution_; });
169 
170  while (!ptcOrSolutionFound())
171  {
172  QuotientSpace *jQuotient = priorityQueue_.top();
173  priorityQueue_.pop();
174  jQuotient->grow();
175 
176  bool hasSolution = quotientSpaces_.at(k)->hasSolution();
177  if (hasSolution)
178  {
179  ompl::base::PathPtr sol_k;
180  quotientSpaces_.at(k)->getSolution(sol_k);
181  solutions_.push_back(sol_k);
182  double t_k_end = ompl::time::seconds(ompl::time::now() - t_start);
183  OMPL_DEBUG("Found Solution on Level %d after %f seconds.", k, t_k_end);
184  foundKLevelSolution_ = true;
185  currentQuotientLevel_ = k + 1;
186 
187  // add solution to pdef
188  ompl::base::PlannerSolution psol(sol_k);
189  std::string lvl_name = getName() + " LvL" + std::to_string(k);
190  psol.setPlannerName(lvl_name);
191  quotientSpaces_.at(k)->getProblemDefinition()->addSolutionPath(psol);
192  }
193  priorityQueue_.push(jQuotient);
194  }
195 
196  if (!foundKLevelSolution_)
197  {
198  OMPL_DEBUG("Planner failed finding solution on QuotientSpace level %d", k);
200  }
201  }
202  double t_end = ompl::time::seconds(ompl::time::now() - t_start);
203  OMPL_DEBUG("Found exact solution after %f seconds.", t_end);
204 
206  if (quotientSpaces_.at(currentQuotientLevel_ - 1)->getSolution(sol))
207  {
208  ompl::base::PlannerSolution psol(sol);
209  psol.setPlannerName(getName());
210  pdef_->addSolutionPath(psol);
211  }
212 
214 }
215 
216 template <class T>
218 ompl::geometric::MultiQuotient<T>::getProblemDefinition(unsigned int kQuotientSpace) const
219 {
220  assert(kQuotientSpace >= 0);
221  assert(kQuotientSpace <= siVec_.size() - 1);
222  return quotientSpaces_.at(kQuotientSpace)->getProblemDefinition();
223 }
224 
225 template <class T>
227 {
228  this->Planner::setProblemDefinition(pdef);
229 
230  // Compute projection of qInit and qGoal onto QuotientSpaces
231  ompl::base::Goal *goal = pdef_->getGoal().get();
232  ompl::base::GoalState *goalRegion = dynamic_cast<ompl::base::GoalState *>(goal);
233  double epsilon = goalRegion->getThreshold();
234  assert(quotientSpaces_.size() == siVec_.size());
235 
236  ompl::base::State *sInit = pdef->getStartState(0);
237  ompl::base::State *sGoal = goalRegion->getState();
238 
239  OMPL_DEVMSG1("Projecting start and goal onto QuotientSpaces.");
240 
241  quotientSpaces_.back()->setProblemDefinition(pdef);
242 
243  for (unsigned int k = siVec_.size() - 1; k > 0; k--)
244  {
245  QuotientSpace *quotientParent = quotientSpaces_.at(k);
246  QuotientSpace *quotientChild = quotientSpaces_.at(k - 1);
248  ompl::base::ProblemDefinitionPtr pdefk = std::make_shared<base::ProblemDefinition>(sik);
249 
250  ompl::base::State *sInitK = sik->allocState();
251  ompl::base::State *sGoalK = sik->allocState();
252 
253  quotientParent->projectQ0(sInit, sInitK);
254  quotientParent->projectQ0(sGoal, sGoalK);
255 
256  pdefk->setStartAndGoalStates(sInitK, sGoalK, epsilon);
257 
258  quotientChild->setProblemDefinition(pdefk);
259 
260  sInit = sInitK;
261  sGoal = sGoalK;
262  }
263 }
264 
265 template <class T>
267 {
268  unsigned int Nvertices = data.numVertices();
269  if (Nvertices > 0)
270  {
271  OMPL_ERROR("PlannerData has %d vertices.", Nvertices);
272  throw ompl::Exception("cannot get planner data if plannerdata is already populated");
273  }
274 
275  unsigned int K = std::min(solutions_.size() + 1, quotientSpaces_.size());
276  K = std::min(K, stopAtLevel_);
277 
278  for (unsigned int k = 0; k < K; k++)
279  {
280  QuotientSpace *Qk = quotientSpaces_.at(k);
281  Qk->getPlannerData(data);
282 
283  // label all new vertices
284  unsigned int ctr = 0;
285  for (unsigned int vidx = Nvertices; vidx < data.numVertices(); vidx++)
286  {
288  static_cast<ompl::base::PlannerDataVertexAnnotated &>(data.getVertex(vidx));
289  v.setLevel(k);
290  v.setMaxLevel(K);
291 
292  ompl::base::State *s_lift = Qk->getSpaceInformation()->cloneState(v.getState());
293  v.setQuotientState(s_lift);
294 
295  for (unsigned int m = k + 1; m < quotientSpaces_.size(); m++)
296  {
297  QuotientSpace *Qm = quotientSpaces_.at(m);
298 
299  if (Qm->getX1() != nullptr)
300  {
301  ompl::base::State *s_X1 = Qm->getX1()->allocState();
302  ompl::base::State *s_Q1 = Qm->getSpaceInformation()->allocState();
303  if (Qm->getX1()->getStateSpace()->getType() == ompl::base::STATE_SPACE_SO3)
304  {
305  s_X1->as<ompl::base::SO3StateSpace::StateType>()->setIdentity();
306  }
307  if (Qm->getX1()->getStateSpace()->getType() == ompl::base::STATE_SPACE_SO2)
308  {
309  s_X1->as<ompl::base::SO2StateSpace::StateType>()->setIdentity();
310  }
311  Qm->mergeStates(s_lift, s_X1, s_Q1);
312  s_lift = Qm->getSpaceInformation()->cloneState(s_Q1);
313 
314  Qm->getX1()->freeState(s_X1);
315  Qm->getQ1()->freeState(s_Q1);
316  }
317  }
318  v.setState(s_lift);
319  ctr++;
320  }
321  Nvertices = data.numVertices();
322  }
323 }
The exception type for ompl.
Definition: Exception.h:47
double getThreshold() const
Get the distance to the goal that is allowed for a state to be considered in the goal region.
Definition: GoalRegion.h:82
Definition of a goal state.
Definition: GoalState.h:49
const State * getState() const
Get the goal state.
Definition: GoalState.cpp:79
Abstract definition of goals.
Definition: Goal.h:63
A shared pointer wrapper for ompl::base::Path.
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
virtual const ompl::base::State * getState() const override
Retrieve the state associated with this vertex.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:129
A shared pointer wrapper for ompl::base::ProblemDefinition.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:68
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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...
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)....
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)
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...
A single quotient-space.
Definition: QuotientSpace.h:49
void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const
Merge a state from Q0 and X1 into a state on Q1 (concatenate)
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()....
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
void projectQ0(const ompl::base::State *q, ompl::base::State *qQ0) const
Quotient Space Projection Operator onto first component ProjectQ0: Q0 \times X1 \rightarrow Q0.
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
Representation of a solution to a planning problem.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62