BundleSpace.h
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 #ifndef OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLE_
40 #define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLE_
41 
42 #include <ompl/base/Planner.h>
43 #include <ompl/base/goals/GoalSampleableRegion.h>
44 #include <ompl/multilevel/datastructures/ProjectionFactory.h>
45 
46 namespace ompl
47 {
51  namespace multilevel
52  {
54 
55  OMPL_CLASS_FORWARD(BundleSpaceMetric);
57  OMPL_CLASS_FORWARD(Projection);
59  OMPL_CLASS_FORWARD(BundleSpacePropagator);
61 
63  class BundleSpace : public ompl::base::Planner
64  {
65  private:
66  using BaseT = ompl::base::Planner;
67  using BaseT::si_; // make it private.
69 
70  // Note: use getBundle(), or getBase()
71  // to access the SpaceInformationPtr
72 
75 
76  public:
89  BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_ = nullptr);
90 
91  virtual ~BundleSpace();
92 
98  ProjectionPtr getProjection() const;
99 
101  // projection.
102  bool makeProjection();
103 
105  // projection)
106  void setProjection(ProjectionPtr projection);
107 
108  virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override;
109 
111  virtual void grow() = 0;
112 
114  virtual bool getSolution(ompl::base::PathPtr &solution) = 0;
115 
116  /* \brief Change the metric to be used on bundle space (default:
117  * intrinsic bundle space metric) */
118  virtual void setMetric(const std::string &sMetric) = 0;
119 
120  /* \brief Change the propagator to be used on bundle space (default:
121  * intrinsic bundle space propagator) */
122  virtual void setPropagator(const std::string &sPropagator) = 0;
123 
124  /* \brief Return a sample from the current datastructure on the
125  * total space*/
126  virtual void sampleFromDatastructure(ompl::base::State *xBase) = 0;
127  /* \brief Return a sample from bundle space (using restriction
128  * sampling) */
129  virtual void sampleBundle(ompl::base::State *xRandom);
130  /* \brief Same as sampleBundle, but return a valid state */
131  bool sampleBundleValid(ompl::base::State *xRandom);
132 
133  /* \brief Check if there exists a solution */
134  virtual bool hasSolution();
135 
137  virtual bool isInfeasible();
138 
140  virtual bool hasConverged();
141 
142  virtual void clear() override;
143  virtual void setup() override;
144 
146  // allocate computational resources)
147  virtual double getImportance() const = 0;
148 
151  ompl::base::State *allocIdentityStateBase() const;
152  ompl::base::State *allocIdentityState(ompl::base::StateSpacePtr) const;
153  void allocIdentityState(ompl::base::State *, ompl::base::StateSpacePtr) const;
154 
156  static void resetCounter();
157 
159  unsigned int getBaseDimension() const;
161  unsigned int getBundleDimension() const;
163  unsigned int getCoDimension() const;
164 
165  const ompl::base::StateSamplerPtr &getBundleSamplerPtr() const;
166  const ompl::base::StateSamplerPtr &getBaseSamplerPtr() const;
167 
169  BundleSpace *getChild() const;
170 
172  void setChild(BundleSpace *child);
173 
175  BundleSpace *getParent() const;
176 
178  void setParent(BundleSpace *parent);
179 
181  bool hasParent() const;
182 
184  bool hasBaseSpace() const;
185 
187  unsigned int getLevel() const;
188 
190  void setLevel(unsigned int);
191 
194  void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const;
195 
197  void lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const;
198 
199  ompl::base::OptimizationObjectivePtr getOptimizationObjectivePtr() const;
200 
202  friend std::ostream &operator<<(std::ostream &, const BundleSpace &);
203 
204  /* \brief Check if the current bundle space is dynamic*/
205  bool isDynamic() const;
206 
207  /* \brief Get pointer to goal region on bundle space */
208  base::GoalSampleableRegion *getGoalPtr() const;
209 
210  private:
212  unsigned int level_{0};
213 
214  //\brief Being on the k-th bundle space, we denote as baseSpace the k-1-th
215  // bundle space (because it locally acts as the base space for the current class)
216  BundleSpace *childBundleSpace_{nullptr};
217 
218  //\brief Being on the k-th bundle space, we denote as parentSpace the k+1-th
219  // bundle space
220  BundleSpace *parentBundleSpace_{nullptr};
221 
222  /* \brief Total space of bundle */
223  ompl::base::SpaceInformationPtr totalSpace_{nullptr};
224 
225  /* \brief Base space of bundle */
226  ompl::base::SpaceInformationPtr baseSpace_{nullptr};
227 
228  /* \brief Pointer to uniform state sampler on total space */
229  ompl::base::StateSamplerPtr Bundle_sampler_;
230  ompl::base::ValidStateSamplerPtr Bundle_valid_sampler_;
231 
233  virtual bool findSection();
234 
237  ProjectionPtr projection_;
238 
239  protected:
241  void checkBundleSpaceMeasure(std::string name, const ompl::base::StateSpacePtr space) const;
242 
244  void sanityChecks() const;
245 
247  virtual void print(std::ostream &out) const;
248 
250  ompl::base::State *xBaseTmp_{nullptr};
252  ompl::base::State *xBundleTmp_{nullptr};
253 
255  static unsigned int counter_;
256 
258  unsigned int id_{0};
259 
261  bool hasSolution_{false};
262 
264  // least once
265  bool firstRun_{true};
266 
268  bool isDynamic_{false};
269 
271  BundleSpaceMetricPtr metric_;
272 
275  BundleSpacePropagatorPtr propagator_;
276  };
277  } // namespace multilevel
278 } // namespace ompl
279 #endif
BundleSpaceMetricPtr metric_
Metric on bundle space.
Definition: BundleSpace.h:373
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
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
Bundle Space contains three primary characters, the bundle space, the base space and the projection.
Definition: BundleSpace.cpp:60
void sanityChecks() const
Check if Bundle-space has correct structure.
bool firstRun_
Variable to check if this bundle space planner has been run at.
Definition: BundleSpace.h:367
Definition of an abstract state.
Definition: State.h:113
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
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
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()....
A shared pointer wrapper for ompl::base::OptimizationObjective.
bool makeProjection()
Given bundle space and base space, try to guess the right.
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()
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
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
unsigned int getBaseDimension() const
Dimension of Base Space.
virtual bool getSolution(ompl::base::PathPtr &solution)=0
Return best solution.
void setLevel(unsigned int)
Change level in hierarchy.
A single Bundle-space.
Definition: BundleSpace.h:131
virtual void grow()=0
Perform an iteration of the planner.
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.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
BundleSpacePropagatorPtr propagator_
Propagator (steering or interpolation) on bundle space. Note: currently just a stub for base::StatePr...
Definition: BundleSpace.h:377
A shared pointer wrapper for ompl::base::StateSpace.
BundleSpace * getParent() const
Return k+1 th bundle space (locally the total space)
A shared pointer wrapper for ompl::base::StateSampler.
virtual double getImportance() const =0
Compute importance of bundle space (to decide where to.
bool hasParent() const
Return if has parent space pointer.
A shared pointer wrapper for ompl::base::ValidStateSampler.
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.
friend std::ostream & operator<<(std::ostream &, const BundleSpace &)
Write class to stream (use as std::cout << *this << std::endl)
void checkBundleSpaceMeasure(std::string name, const ompl::base::StateSpacePtr space) const
Check if Bundle-space is bounded.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21