BundleSpaceSequenceImpl.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 #include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
39 #include <ompl/base/goals/GoalSampleableRegion.h>
40 #include <ompl/base/goals/GoalState.h>
41 #include <ompl/base/goals/GoalStates.h>
42 #include <ompl/util/Exception.h>
43 #include <ompl/util/Time.h>
44 #include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
45 #include <ompl/multilevel/datastructures/Projection.h>
46 
47 template <class T>
49  : BaseT(si, type)
50 {
51  declareBundleSpaces(true);
52 }
53 
54 template <class T>
55 ompl::multilevel::BundleSpaceSequence<T>::BundleSpaceSequence(std::vector<ompl::base::SpaceInformationPtr> &siVec,
56  std::string type)
57  : BaseT(siVec, type)
58 {
59  declareBundleSpaces(true);
60 }
61 
62 template <class T>
63 ompl::multilevel::BundleSpaceSequence<T>::BundleSpaceSequence(std::vector<ompl::base::SpaceInformationPtr> &siVec,
64  std::vector<ompl::multilevel::ProjectionPtr> &projVec,
65  std::string type)
66  : BaseT(siVec, type)
67 {
68  assert(siVec.size() > 0);
69  assert((siVec.size() - 1) == projVec.size());
70  declareBundleSpaces(false);
71 
72  // None projection (from last state to empty)
73  bundleSpaces_.front()->makeProjection();
74  for (unsigned int k = 1; k < bundleSpaces_.size(); k++)
75  {
76  BundleSpace *bk = bundleSpaces_.at(k);
77  bk->setProjection(projVec.at(k - 1));
78  // need to precompute the location helper functions to utilize
79  //"copyToReals" inside the projection function
80  bk->getBundle()->setup();
81  }
82 }
83 
84 template <class T>
86 {
87  T::resetCounter();
88  for (unsigned int k = 0; k < siVec_.size(); k++)
89  {
90  T *parent = nullptr;
91  if (k > 0)
92  parent = bundleSpaces_.back();
93 
94  T *ss = new T(siVec_.at(k), parent);
95  bundleSpaces_.push_back(ss);
96  static_cast<BundleSpace *>(bundleSpaces_.back())->setLevel(k);
97  }
98  stopAtLevel_ = bundleSpaces_.size();
99 
100  if (guessProjection)
101  {
102  for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
103  {
104  bundleSpaces_.at(k)->makeProjection();
105  }
106  }
107 
108  OMPL_DEBUG("Created %d BundleSpace levels (%s).", siVec_.size(), getName().c_str());
109 }
110 
111 template <class T>
113 {
114  for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
115  {
116  if (bundleSpaces_.at(k))
117  {
118  delete bundleSpaces_.at(k);
119  }
120  }
121  bundleSpaces_.clear();
122 }
123 
124 template <class T>
126 {
127  if (level_ > bundleSpaces_.size())
128  {
129  stopAtLevel_ = bundleSpaces_.size();
130  }
131  else
132  {
133  stopAtLevel_ = level_;
134  }
135 }
136 
137 template <class T>
139 {
140  for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
141  {
142  BundleSpaceGraph *bsg = dynamic_cast<BundleSpaceGraph *>(bundleSpaces_.at(k));
143  if (bsg != nullptr)
144  {
145  bsg->setFindSectionStrategy(type);
146  }
147  }
148 }
149 
150 template <class T>
152 {
153  BaseT::setup();
154  for (unsigned int k = 0; k < stopAtLevel_; k++)
155  {
156  static_cast<BundleSpace *>(bundleSpaces_.at(k))->setup();
157  }
158  currentBundleSpaceLevel_ = 0;
159 }
160 
161 template <class T>
163 {
164  BaseT::clear();
165 
166  for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
167  {
168  static_cast<BundleSpace *>(bundleSpaces_.at(k))->clear();
169  }
170  currentBundleSpaceLevel_ = 0;
171 
172  while (!priorityQueue_.empty())
173  priorityQueue_.pop();
174 
175  foundKLevelSolution_ = false;
176 }
177 
178 template <class T>
181 {
183 
184  for (unsigned int k = currentBundleSpaceLevel_; k < stopAtLevel_; k++)
185  {
186  BundleSpace *kBundle = static_cast<BundleSpace *>(bundleSpaces_.at(k));
187 
188  foundKLevelSolution_ = false;
189 
190  if (priorityQueue_.size() <= currentBundleSpaceLevel_)
191  priorityQueue_.push(kBundle);
192 
193  ompl::base::PlannerTerminationCondition ptcOrSolutionFound(
194  [this, &ptc] { return ptc || foundKLevelSolution_; });
195 
196  while (!ptcOrSolutionFound())
197  {
198  BundleSpace *jBundle = priorityQueue_.top();
199  priorityQueue_.pop();
200  jBundle->grow();
201 
202  bool hasSolution = kBundle->hasSolution();
203  if (hasSolution)
204  {
205  ompl::base::PathPtr sol_k;
206  kBundle->getSolution(sol_k);
207  if (solutions_.size() < k + 1)
208  {
209  solutions_.push_back(sol_k);
210  double t_k_end = ompl::time::seconds(ompl::time::now() - t_start);
211  OMPL_DEBUG("Found Solution on Level %d/%d after %f seconds.", k + 1, stopAtLevel_, t_k_end);
212  currentBundleSpaceLevel_ = k + 1;
213  if (currentBundleSpaceLevel_ > (bundleSpaces_.size() - 1))
214  currentBundleSpaceLevel_ = bundleSpaces_.size() - 1;
215  }
216  else
217  {
218  solutions_.at(k) = sol_k;
219  }
220  foundKLevelSolution_ = true;
221 
222  // add solution to pdef
223  ompl::base::PlannerSolution psol(sol_k);
224  std::string lvl_name = getName() + " LvL" + std::to_string(k);
225  psol.setPlannerName(lvl_name);
226 
227  kBundle->getProblemDefinition()->clearSolutionPaths();
228  kBundle->getProblemDefinition()->addSolutionPath(psol);
229  }
230 
231  bool isInfeasible = kBundle->isInfeasible();
232  if (isInfeasible)
233  {
234  double t_end = ompl::time::seconds(ompl::time::now() - t_start);
235  OMPL_DEBUG("Infeasibility detected after %f seconds (level %d).", t_end, k);
237  }
238  priorityQueue_.push(jBundle);
239  }
240 
241  if (!foundKLevelSolution_)
242  {
243  OMPL_DEBUG("-- Planner failed finding solution on BundleSpace level %d", k);
245  }
246  }
247 
249  ompl::base::PlannerSolution psol(sol);
250  static_cast<BundleSpace *>(bundleSpaces_.back())->getProblemDefinition()->getSolution(psol);
251  pdef_->addSolutionPath(psol);
252 
254 }
255 
256 template <class T>
258 {
259  BaseT::setProblemDefinition(pdef);
260 
261  if (siVec_.size() < 1)
262  return;
263 
264  pdefVec_.clear();
265  pdefVec_.push_back(pdef);
266  bundleSpaces_.back()->setProblemDefinition(pdef);
267 
268  if (siVec_.size() <= 1)
269  return;
270 
271  assert(bundleSpaces_.size() == siVec_.size());
272 
273  //#########################################################################
274  // Check if goal type is projectable
275  //#########################################################################
276  ompl::base::GoalType type = pdef_->getGoal()->getType();
277  if (!(type == ompl::base::GoalType::GOAL_STATE || type == ompl::base::GoalType::GOAL_STATES))
278  {
279  OMPL_ERROR("If you want to use other goal classes than \"GoalSampleableRegion\", you need to specify them "
280  "manually for each SpaceInformationPtr in the hierarchy.");
281  throw ompl::Exception("Multilevel framework does not support provided goal specs.");
282  }
283 
284  //#########################################################################
285  // Iterate through all levels and project from the last level down
286  //#########################################################################
288  static_cast<ompl::base::GoalSampleableRegion *>(pdef_->getGoal().get());
289  double epsilon = goalRegion->getThreshold();
290 
291  base::OptimizationObjectivePtr obj = pdef->getOptimizationObjective();
292 
293  for (unsigned int k = siVec_.size() - 1; k > 0; k--)
294  {
295  BundleSpace *parent = static_cast<BundleSpace *>(bundleSpaces_.at(k));
296  BundleSpace *child = static_cast<BundleSpace *>(bundleSpaces_.at(k - 1));
297  ompl::base::SpaceInformationPtr siChild = child->getBundle();
298 
299  ompl::base::ProblemDefinitionPtr pdefParent = pdefVec_.back();
300 
301  ompl::base::ProblemDefinitionPtr pdefChild = std::make_shared<base::ProblemDefinition>(siChild);
302 
303  // Project Start State onto lower dimensional quotient space
304  const ompl::base::State *sInitParent = pdefParent->getStartState(0);
305  ompl::base::State *sInitChild = siChild->allocState();
306 
307  parent->getProjection()->project(sInitParent, sInitChild);
308  parent->getProjection()->project(sInitParent, sInitChild);
309  pdefChild->addStartState(sInitChild);
310 
311  // Now project goal state(s) down
312  if (type == ompl::base::GoalType::GOAL_STATE)
313  {
314  ompl::base::GoalState *goal = static_cast<ompl::base::GoalState *>(pdefParent->getGoal().get());
315 
316  const ompl::base::State *sGoalParent = goal->getState();
317  ompl::base::State *sGoalChild = siChild->allocState();
318  parent->getProjection()->project(sGoalParent, sGoalChild);
319  pdefChild->setGoalState(sGoalChild, epsilon);
320  }
321  else if (type == ompl::base::GoalType::GOAL_STATES)
322  {
323  ompl::base::GoalStates *goal = static_cast<ompl::base::GoalStates *>(pdefParent->getGoal().get());
324  unsigned int N = goal->getStateCount();
325 
326  ompl::base::GoalStatesPtr goalStates = std::make_shared<ompl::base::GoalStates>(siChild);
327  goalStates->setThreshold(epsilon);
328 
329  for (unsigned int j = 0; j < N; j++)
330  {
331  const ompl::base::State *sGoalParent = goal->getState(j);
332  ompl::base::State *sGoalChild = siChild->allocState();
333  parent->getProjection()->project(sGoalParent, sGoalChild);
334  goalStates->addState(sGoalChild);
335  }
336  pdefChild->setGoal(goalStates);
337  }
338 
339  child->setProblemDefinition(pdefChild);
340  pdefVec_.push_back(pdefChild);
341  }
342 
343  std::reverse(pdefVec_.begin(), pdefVec_.end());
344 }
345 
346 template <class T>
348  const base::State *baseState) const
349 {
350  BundleSpace *Qprev = bundleSpaces_.at(baseLevel);
351  ompl::base::State *s_lift = Qprev->getBundle()->cloneState(baseState);
352 
353  for (unsigned int m = baseLevel + 1; m < bundleSpaces_.size(); m++)
354  {
355  BundleSpace *Qm = bundleSpaces_.at(m);
356 
357  if (Qm->getProjection()->getCoDimension() > 0)
358  {
359  base::State *s_Bundle = Qm->allocIdentityStateBundle();
360 
361  Qm->getProjection()->lift(s_lift, s_Bundle);
362 
363  Qprev->getBundle()->freeState(s_lift);
364 
365  s_lift = Qm->getBundle()->cloneState(s_Bundle);
366 
367  Qm->getBundle()->freeState(s_Bundle);
368 
369  Qprev = Qm;
370  }
371  }
372  return s_lift;
373 }
374 
375 template <class T>
377 {
378  unsigned int Nvertices = data.numVertices();
379  if (Nvertices > 0)
380  {
381  OMPL_ERROR("PlannerData has %d vertices.", Nvertices);
382  throw ompl::Exception("cannot get planner data if plannerdata is already populated");
383  }
384 
385  unsigned int K = std::min(solutions_.size() + 1, bundleSpaces_.size());
386  K = std::min(K, stopAtLevel_);
387 
388  BundleSpace *Qlast = this->bundleSpaces_.back();
389  for (unsigned int k = 0; k < K; k++)
390  {
391  BundleSpace *Qk = static_cast<BundleSpace *>(bundleSpaces_.at(k));
392  Qk->getPlannerData(data);
393 
394  // lift all states into the last bundle space (original state space)
395  // Required for decouplePlannerData() function in PlannerData
396 
397  unsigned int ctr = 0;
398 
399  for (unsigned int vidx = Nvertices; vidx < data.numVertices(); vidx++)
400  {
402  static_cast<ompl::multilevel::PlannerDataVertexAnnotated &>(data.getVertex(vidx));
403  v.setLevel(k);
404  v.setMaxLevel(K);
405 
406  base::State *s_lift = getTotalState(k, v.getBaseState());
407  v.setTotalState(s_lift, Qlast->getBundle());
408  ctr++;
409  }
410  Nvertices = data.numVertices();
411  }
412  OMPL_DEBUG("Multilevel Graph has %d/%d vertices/edges", data.numVertices(), data.numEdges());
413 }
void setMaxLevel(unsigned int level_)
The maximum level in the bundle space hierarchy.
ompl::base::State * getTotalState(int baseLevel, const base::State *baseState) const
Starting from a baseState on baseLevel, we lift it iteratively upwards into the total space of the se...
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
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...
const ompl::base::State * getBaseState() const
Returns base state, indepent of mode.
Definition of an abstract state.
Definition: State.h:113
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 multilevel hierarchy....
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()....
Representation of a solution to a planning problem.
point now()
Get the current time point.
Definition: Time.h:122
std::vector< T * > bundleSpaces_
Sequence of BundleSpaces.
unsigned int numEdges() const
Retrieve the number of edges in this structure.
@ TIMEOUT
The planner failed to find a solution.
bool makeProjection()
Given bundle space and base space, try to guess the right.
Definition of a set of goal states.
Definition: GoalStates.h:113
A graph on a Bundle-space.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void setTotalState(ompl::base::State *s, ompl::base::SpaceInformationPtr si)
Set total state, i.e. the lift of the base state to the total space (last Spaceinformationptr in sequ...
BundleSpaceSequence(ompl::base::SpaceInformationPtr si, std::string type="BundleSpacePlannerNonMultilevel")
Non-multilevel Mode: Calling with a single ompl::base::SpaceInformationPtr will revert to standard pl...
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
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.
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about BundleSpace level)
virtual void clear() override
Clear multilevel planner by clearing all levels.
A class to store the exit status of Planner::solve()
void setProjection(ProjectionPtr projection)
Set explicit projection (so that we do not need to guess.
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,...
@ INFEASIBLE
The planner decided that problem is infeasible.
virtual bool getSolution(ompl::base::PathPtr &solution)=0
Return best solution.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
@ EXACT_SOLUTION
The planner found an exact solution.
void setLevel(unsigned int level_)
The level of vertex in the bundle space hierarchy.
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 single Bundle-space.
Definition: BundleSpace.h:131
virtual void grow()=0
Perform an iteration of the planner.
Definition of a goal state.
Definition: GoalState.h:112
MultiLevel Planner Interface. Extends base::Planner by allowing sequences of base::SpaceInformationPt...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
GoalType
The type of goal.
Definition: GoalTypes.h:109
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.
ompl::base::State * allocIdentityStateBundle() const
Allocate State, set entries to Identity/Zero.
Abstract definition of a goal region that can be sampled.
virtual bool isInfeasible()
Check if any infeasibility guarantees are fulfilled.
A planner for a sequence of BundleSpaces.
The exception type for ompl.
Definition: Exception.h:78
void setFindSectionStrategy(FindSectionType type)
Set strategy to use to solve the find section problem.
#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
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71