Loading...
Searching...
No Matches
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
46namespace ompl
47{
51 namespace multilevel
52 {
54
55 OMPL_CLASS_FORWARD(BundleSpaceMetric);
57 OMPL_CLASS_FORWARD(Projection);
59 OMPL_CLASS_FORWARD(BundleSpacePropagator);
61
64 {
65 private:
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
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
Abstract definition of a goal region that can be sampled.
A shared pointer wrapper for ompl::base::OptimizationObjective.
A shared pointer wrapper for ompl::base::Path.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition Planner.cpp:66
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::base::ValidStateSampler.
A single Bundle-space.
Definition BundleSpace.h:64
static unsigned int counter_
Internal counter to track multiple bundle spaces.
void lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const
Lift a state from Base to Bundle.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
bool isDynamic_
If the problem is dynamic or geometric.
void setLevel(unsigned int)
Change level in hierarchy.
bool hasParent() const
Return if has parent space pointer.
BundleSpacePropagatorPtr propagator_
Propagator (steering or interpolation) on bundle space. Note: currently just a stub for base::StatePr...
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
unsigned int getBaseDimension() const
Dimension of Base Space.
virtual bool isInfeasible()
Check if any infeasibility guarantees are fulfilled.
void setProjection(ProjectionPtr projection)
Set explicit projection (so that we do not need to guess.
virtual void grow()=0
Perform an iteration of the planner.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
void setParent(BundleSpace *parent)
Pointer to k+1 th bundle space (locally the total space)
unsigned int id_
Identity of space (to keep track of number of Bundle-spaces created)
virtual bool getSolution(ompl::base::PathPtr &solution)=0
Return best solution.
bool hasSolution_
If there exists a solution.
ompl::base::State * xBaseTmp_
A temporary state on Base.
BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
Bundle Space contains three primary characters, the bundle space, the base space and the projection.
BundleSpaceMetricPtr metric_
Metric on bundle space.
virtual double getImportance() const =0
Compute importance of bundle space (to decide where to.
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
static void resetCounter()
reset counter for number of levels
bool makeProjection()
Given bundle space and base space, try to guess the right.
ompl::base::State * allocIdentityStateBundle() const
Allocate State, set entries to Identity/Zero.
friend std::ostream & operator<<(std::ostream &, const BundleSpace &)
Write class to stream (use as std::cout << *this << std::endl)
BundleSpace * getParent() const
Return k+1 th bundle space (locally the total space)
bool hasBaseSpace() const
Return if has base space pointer.
unsigned int getBundleDimension() const
Dimension of Bundle Space.
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()....
virtual bool hasConverged()
Check if the current space can still be sampled.
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
bool firstRun_
Variable to check if this bundle space planner has been run at.
void setChild(BundleSpace *child)
Pointer to k-1 th bundle space (locally the base space)
void sanityChecks() const
Check if Bundle-space has correct structure.
void checkBundleSpaceMeasure(std::string name, const ompl::base::StateSpacePtr space) const
Check if Bundle-space is bounded.
ompl::base::State * xBundleTmp_
A temporary state on Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
unsigned int getLevel() const
Level in hierarchy of Bundle-spaces.
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()