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 }
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...
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
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)
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)
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.
Definition of an abstract state.
Definition: State.h:114
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
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.
Representation of a solution to a planning problem.
point now()
Get the current time point.
Definition: Time.h:122
The definition of a state in SO(3) represented as a unit quaternion.
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ TIMEOUT
The planner failed to find a solution.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
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 numVertices() const
Retrieve the number of vertices in this structure.
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()
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()....
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
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.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
The definition of a state in SO(2)
Abstract definition of goals.
Definition: Goal.h:127
std::vector< ompl::base::SpaceInformationPtr > siVec_
Each QuotientSpace has a unique ompl::base::SpaceInformationPtr.
const State * getState() const
Get the goal state.
Definition: GoalState.cpp:79
@ EXACT_SOLUTION
The planner found an exact solution.
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
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)
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
Definition of a goal state.
Definition: GoalState.h:113
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
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:178
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
virtual const ompl::base::State * getState() const override
Retrieve the state associated with this vertex.
A single quotient-space.
The exception type for ompl.
Definition: Exception.h:79
void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about QuotientSpace level)
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
Main namespace. Contains everything in this library.
Definition: AppBase.h:22