BundleSpace.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020,
5  * Max Planck Institute for Intelligent Systems (MPI-IS).
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the MPI-IS nor the names
19  * of its contributors may be used to endorse or promote products
20  * derived from this software without specific prior written
21  * permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Andreas Orthey */
38 
39 #include <ompl/multilevel/datastructures/BundleSpace.h>
40 #include <ompl/multilevel/datastructures/Projection.h>
41 
42 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
43 #include <ompl/base/goals/GoalSampleableRegion.h>
44 #include <ompl/control/SpaceInformation.h>
45 #include <ompl/base/spaces/SO2StateSpace.h>
46 #include <ompl/base/spaces/SO3StateSpace.h>
47 #include <ompl/base/spaces/RealVectorStateSpace.h>
48 #include <ompl/base/spaces/TimeStateSpace.h>
49 #include <ompl/base/spaces/DiscreteStateSpace.h>
50 #include <ompl/tools/config/MagicConstants.h>
51 
52 #include <ompl/util/Exception.h>
53 #include <cmath> //to use isnan(d)
54 
55 using namespace ompl::base;
56 using namespace ompl::multilevel;
57 
58 unsigned int BundleSpace::counter_ = 0;
59 
60 BundleSpace::BundleSpace(const SpaceInformationPtr &si, BundleSpace *child)
61  : Planner(si, "BundleSpace"), childBundleSpace_(child), totalSpace_(si)
62 {
63  id_ = counter_++;
64  if (child)
65  {
66  baseSpace_ = childBundleSpace_->getBundle();
67  childBundleSpace_->setParent(this);
68  xBaseTmp_ = getBase()->allocState();
69  }
70 
71  std::stringstream ss;
72  ss << (*this);
73  OMPL_DEBUG(ss.str().c_str());
74 
75  if (!Bundle_valid_sampler_)
76  {
77  Bundle_valid_sampler_ = getBundle()->allocValidStateSampler();
78  }
79  if (!Bundle_sampler_)
80  {
81  Bundle_sampler_ = getBundle()->allocStateSampler();
82  }
83  xBundleTmp_ = getBundle()->allocState();
84 }
85 
86 BundleSpace::~BundleSpace()
87 {
88  if (hasBaseSpace())
89  {
90  if (xBaseTmp_)
91  {
92  getBase()->freeState(xBaseTmp_);
93  }
94  }
95  if (xBundleTmp_)
96  {
97  getBundle()->freeState(xBundleTmp_);
98  }
99 }
100 
102 {
103  ProjectionFactory projectionFactory;
104 
105  projection_ = projectionFactory.makeProjection(getBundle(), getBase());
106 
107  if (!projection_)
108  return false;
109 
110  sanityChecks();
111  return true;
112 }
113 
115 {
116  return !(baseSpace_ == nullptr);
117 }
118 
119 bool BundleSpace::findSection()
120 {
121  return false;
122 }
123 
125 {
126  return !(parentBundleSpace_ == nullptr);
127 }
128 
129 bool BundleSpace::isDynamic() const
130 {
131  return isDynamic_;
132 }
133 
135 {
136  BaseT::setup();
137 
138  hasSolution_ = false;
139  firstRun_ = true;
140 
141  if (pdef_)
142  {
143  if (!pdef_->hasOptimizationObjective())
144  {
145  OptimizationObjectivePtr lengthObj = std::make_shared<base::PathLengthOptimizationObjective>(getBundle());
146 
147  lengthObj->setCostThreshold(base::Cost(std::numeric_limits<double>::infinity()));
148  pdef_->setOptimizationObjective(lengthObj);
149  }
150  }
151  else
152  {
153  OMPL_ERROR("Called without ProblemDefinitionPtr");
154  throw "NoProblemDef";
155  }
156 }
157 
158 GoalSampleableRegion *BundleSpace::getGoalPtr() const
159 {
160  base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
161  return goal;
162 }
163 
165 {
166  BaseT::clear();
167 
168  hasSolution_ = false;
169  firstRun_ = true;
170 
171  pdef_->clearSolutionPaths();
172 }
173 
175 {
176  const StateSpacePtr Bundle_space = getBundle()->getStateSpace();
177  checkBundleSpaceMeasure("Bundle", Bundle_space);
178 
179  if (hasBaseSpace())
180  {
181  const StateSpacePtr Base_space = getBase()->getStateSpace();
182  checkBundleSpaceMeasure("Base", Base_space);
183  if (getProjection()->getDimension() != getBundleDimension())
184  {
185  throw Exception("BundleSpace Dimensions are wrong.");
186  }
187  }
188 }
189 
190 void BundleSpace::checkBundleSpaceMeasure(std::string name, const StateSpacePtr space) const
191 {
192  OMPL_DEVMSG1("%s dimension: %d measure: %f", name.c_str(), space->getDimension(), space->getMeasure());
193 
194  if ((space->getMeasure() >= std::numeric_limits<double>::infinity()))
195  {
196  throw Exception("Space infinite measure.");
197  }
198 }
199 
200 PlannerStatus BundleSpace::solve(const PlannerTerminationCondition &)
201 {
202  throw Exception("A Bundle-Space cannot be solved alone. \
203  Use class BundleSpaceSequence to solve Bundle-Spaces.");
204 }
205 
207 {
209 }
210 
212 {
214 }
215 
216 void BundleSpace::setProjection(ProjectionPtr projection)
217 {
218  projection_ = projection;
219  if (getProjection() == nullptr)
220  {
221  OMPL_ERROR("Projection is nullptr.");
222  throw "Projection is nullptr.";
223  }
224 }
225 
226 ProjectionPtr BundleSpace::getProjection() const
227 {
228  return projection_;
229 }
230 
231 void BundleSpace::allocIdentityState(State *s, StateSpacePtr space) const
232 {
233  if (space->isCompound())
234  {
235  CompoundStateSpace *cspace = space->as<CompoundStateSpace>();
236  const std::vector<StateSpacePtr> compounds = cspace->getSubspaces();
237  for (unsigned int k = 0; k < compounds.size(); k++)
238  {
239  StateSpacePtr spacek = compounds.at(k);
240  State *xk = s->as<CompoundState>()->as<State>(k);
241  allocIdentityState(xk, spacek);
242  }
243  }
244  else
245  {
246  int stype = space->getType();
247  switch (stype)
248  {
249  case STATE_SPACE_SO3:
250  {
251  static_cast<SO3StateSpace::StateType *>(s)->setIdentity();
252  break;
253  }
254  case STATE_SPACE_SO2:
255  {
256  static_cast<SO2StateSpace::StateType *>(s)->setIdentity();
257  break;
258  }
259  case STATE_SPACE_TIME:
260  {
261  static_cast<TimeStateSpace::StateType *>(s)->position = 0;
262  break;
263  }
265  {
266  DiscreteStateSpace *space_Discrete = space->as<DiscreteStateSpace>();
267  int lb = space_Discrete->getLowerBound();
268  static_cast<DiscreteStateSpace::StateType *>(s)->value = lb;
269  break;
270  }
272  {
274  RealVectorStateSpace *RN = space->as<RealVectorStateSpace>();
275  const std::vector<double> &bl = RN->getBounds().low;
276  const std::vector<double> &bh = RN->getBounds().high;
277  for (unsigned int k = 0; k < space->getDimension(); k++)
278  {
279  double &v = sRN->values[k];
280  v = 0.0;
281 
282  // if zero is not valid, use mid point as identity
283  if (v < bl.at(k) || v > bh.at(k))
284  {
285  v = bl.at(k) + 0.5 * (bh.at(k) - bl.at(k));
286  }
287  }
288  break;
289  }
290  default:
291  {
292  OMPL_ERROR("Type: %d not recognized.", stype);
293  throw Exception("Type not recognized.");
294  }
295  }
296  }
297 }
298 
299 State *BundleSpace::allocIdentityState(StateSpacePtr space) const
300 {
301  if (space != nullptr)
302  {
303  State *s = space->allocState();
304  allocIdentityState(s, space);
305  return s;
306  }
307  else
308  {
309  return nullptr;
310  }
311 }
312 
314 {
315  return allocIdentityState(getBundle()->getStateSpace());
316 }
317 
318 State *BundleSpace::allocIdentityStateBase() const
319 {
320  return allocIdentityState(getBase()->getStateSpace());
321 }
322 
324 {
325  return totalSpace_;
326 }
327 
329 {
330  return baseSpace_;
331 }
332 
333 unsigned int BundleSpace::getBaseDimension() const
334 {
335  if (getBase())
336  return getBase()->getStateDimension();
337  else
338  return 0;
339 }
340 
342 {
343  return getBundle()->getStateDimension();
344 }
345 
346 unsigned int BundleSpace::getCoDimension() const
347 {
349 }
350 
351 const StateSamplerPtr &BundleSpace::getBaseSamplerPtr() const
352 {
353  if (hasBaseSpace())
354  {
355  return getChild()->getBundleSamplerPtr();
356  }
357  else
358  {
359  OMPL_ERROR("Cannot get Base Sampler without Base Space.");
360  throw Exception("Tried Calling Non-existing base space sampler");
361  }
362 }
363 
364 const StateSamplerPtr &BundleSpace::getBundleSamplerPtr() const
365 {
366  return Bundle_sampler_;
367 }
368 
370 {
371  return false;
372 }
373 
375 {
376  return false;
377 }
378 
379 bool BundleSpace::hasSolution()
380 {
381  if (!hasSolution_)
382  {
383  PathPtr path;
384  hasSolution_ = getSolution(path);
385  }
386  return hasSolution_;
387 }
388 
390 {
391  return childBundleSpace_;
392 }
393 
395 {
396  childBundleSpace_ = child;
397 }
398 
400 {
401  return parentBundleSpace_;
402 }
403 
405 {
406  parentBundleSpace_ = parent;
407 }
408 
409 unsigned int BundleSpace::getLevel() const
410 {
411  return level_;
412 }
413 
414 void BundleSpace::setLevel(unsigned int level)
415 {
416  level_ = level;
417 }
418 
419 OptimizationObjectivePtr BundleSpace::getOptimizationObjectivePtr() const
420 {
421  return pdef_->getOptimizationObjective();
422 }
423 
424 bool BundleSpace::sampleBundleValid(State *xRandom)
425 {
426  bool found = false;
427 
428  unsigned int attempts = 0;
429  do
430  {
431  sampleBundle(xRandom);
432  found = getBundle()->getStateValidityChecker()->isValid(xRandom);
433  attempts++;
434  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
435  return found;
436 }
437 
438 void BundleSpace::sampleBundle(State *xRandom)
439 {
440  if (!hasBaseSpace())
441  {
442  Bundle_sampler_->sampleUniform(xRandom);
443  }
444  else
445  {
446  if (getProjection()->getCoDimension() > 0)
447  {
448  // Adjusted sampling function: Sampling in G0 x Fiber
449  getChild()->sampleFromDatastructure(xBaseTmp_);
450  getProjection()->lift(xBaseTmp_, xRandom);
451  }
452  else
453  {
454  getChild()->sampleFromDatastructure(xRandom);
455  }
456  }
457 }
458 
459 void BundleSpace::lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const
460 {
461  projection_->lift(xBase, xBundle);
462 }
463 
464 void BundleSpace::project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
465 {
466  projection_->project(xBundle, xBase);
467 }
468 
469 void BundleSpace::print(std::ostream &out) const
470 {
471  out << getProjection();
472 }
473 
474 namespace ompl
475 {
476  namespace multilevel
477  {
478  std::ostream &operator<<(std::ostream &out, const BundleSpace &bundleSpace)
479  {
480  bundleSpace.print(out);
481  return out;
482  }
483  }
484 }
Definition of a compound state.
Definition: State.h:150
ompl::base::State * xBundleTmp_
A temporary state on Bundle.
Definition: BundleSpace.h:354
static unsigned int counter_
Internal counter to track multiple bundle spaces.
Definition: BundleSpace.h:357
Base class for a planner.
Definition: Planner.h:279
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition: Cost.cpp:39
A shared pointer wrapper for ompl::base::Path.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
void sanityChecks() const
Check if Bundle-space has correct structure.
A space to allow the composition of state spaces.
Definition: StateSpace.h:637
A space representing discrete states; i.e. there are a small number of discrete states the system can...
bool firstRun_
Variable to check if this bundle space planner has been run at.
Definition: BundleSpace.h:367
A shared pointer wrapper for ompl::control::SpaceInformation.
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::vector< double > low
Lower bound.
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:477
The definition of a discrete state.
void setParent(BundleSpace *parent)
Pointer to k+1 th bundle space (locally the total space)
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
unsigned int id_
Identity of space (to keep track of number of Bundle-spaces created)
Definition: BundleSpace.h:360
double * values
The value of the actual vector in Rn
This namespace contains datastructures and planners to exploit multilevel abstractions,...
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()....
The definition of a state in SO(3) represented as a unit quaternion.
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
A shared pointer wrapper for ompl::base::OptimizationObjective.
bool makeProjection()
Given bundle space and base space, try to guess the right.
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
A state space representing Rn. The distance function is the L2 norm.
int getLowerBound() const
Returns the lowest possible state.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
void setChild(BundleSpace *child)
Pointer to k-1 th bundle space (locally the base space)
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
static void resetCounter()
reset counter for number of levels
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
bool hasSolution_
If there exists a solution.
Definition: BundleSpace.h:363
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
virtual bool hasConverged()
Check if the current space can still be sampled.
unsigned int getBundleDimension() const
Dimension of Bundle Space.
A class to store the exit status of Planner::solve()
@ STATE_SPACE_DISCRETE
ompl::base::DiscreteStateSpace
void setProjection(ProjectionPtr projection)
Set explicit projection (so that we do not need to guess.
A shared pointer wrapper for ompl::base::ProblemDefinition.
bool hasBaseSpace() const
Return if has base space pointer.
bool isDynamic_
If the problem is dynamic or geometric.
Definition: BundleSpace.h:370
The definition of a time state.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
The definition of a state in SO(2)
unsigned int getBaseDimension() const
Dimension of Base Space.
virtual bool getSolution(ompl::base::PathPtr &solution)=0
Return best solution.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:118
void setLevel(unsigned int)
Change level in hierarchy.
virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()....
Definition: Planner.cpp:81
ProjectionPtr makeProjection(const base::SpaceInformationPtr &Bundle, const base::SpaceInformationPtr &Base)
Guess projection(s) between two SpaceInformationPtr Bundle and Base.
A single Bundle-space.
Definition: BundleSpace.h:131
ompl::base::State * xBaseTmp_
A temporary state on Base.
Definition: BundleSpace.h:352
void lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const
Lift a state from Base to Bundle.
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ STATE_SPACE_TIME
ompl::base::TimeStateSpace
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
BundleSpace * getParent() const
Return k+1 th bundle space (locally the total space)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::StateSampler.
bool hasParent() const
Return if has parent space pointer.
ompl::base::State * allocIdentityStateBundle() const
Allocate State, set entries to Identity/Zero.
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
Abstract definition of a goal region that can be sampled.
virtual bool isInfeasible()
Check if any infeasibility guarantees are fulfilled.
The exception type for ompl.
Definition: Exception.h:78
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void checkBundleSpaceMeasure(std::string name, const ompl::base::StateSpacePtr space) const
Check if Bundle-space is bounded.
Main namespace. Contains everything in this library.