Loading...
Searching...
No Matches
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
55using namespace ompl::base;
56using namespace ompl::multilevel;
57
58unsigned int BundleSpace::counter_ = 0;
59
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
86BundleSpace::~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
119bool BundleSpace::findSection()
120{
121 return false;
122}
123
125{
126 return !(parentBundleSpace_ == nullptr);
127}
128
129bool 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
158GoalSampleableRegion *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
190void 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
200PlannerStatus 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
210
215
216void 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
226ProjectionPtr BundleSpace::getProjection() const
227{
228 return projection_;
229}
230
231void 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 {
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
299State *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
318State *BundleSpace::allocIdentityStateBase() const
319{
320 return allocIdentityState(getBase()->getStateSpace());
321}
322
324{
325 return totalSpace_;
326}
327
329{
330 return baseSpace_;
331}
332
334{
335 if (getBase())
336 return getBase()->getStateDimension();
337 else
338 return 0;
339}
340
342{
343 return getBundle()->getStateDimension();
344}
345
346unsigned int BundleSpace::getCoDimension() const
347{
349}
350
351const 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
364const StateSamplerPtr &BundleSpace::getBundleSamplerPtr() const
365{
366 return Bundle_sampler_;
367}
368
370{
371 return false;
372}
373
375{
376 return false;
377}
378
379bool BundleSpace::hasSolution()
380{
381 if (!hasSolution_)
382 {
383 PathPtr 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
409unsigned int BundleSpace::getLevel() const
410{
411 return level_;
412}
413
414void BundleSpace::setLevel(unsigned int level)
415{
416 level_ = level;
417}
418
419OptimizationObjectivePtr BundleSpace::getOptimizationObjectivePtr() const
420{
421 return pdef_->getOptimizationObjective();
422}
423
424bool 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++;
435 return found;
436}
437
438void 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
459void BundleSpace::lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const
460{
461 projection_->lift(xBase, xBundle);
462}
463
465{
466 projection_->project(xBundle, xBase);
467}
468
469void BundleSpace::print(std::ostream &out) const
470{
471 out << getProjection();
472}
473
474namespace 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}
The exception type for ompl.
Definition Exception.h:47
A space to allow the composition of state spaces.
Definition StateSpace.h:574
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition of a compound state.
Definition State.h:87
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
The definition of a discrete state.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
int getLowerBound() const
Returns the lowest possible state.
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
T * as()
Cast this instance to a desired type.
Definition Planner.h:230
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition Planner.cpp:118
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition Planner.cpp:92
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
A shared pointer wrapper for ompl::base::ProblemDefinition.
std::vector< double > low
Lower bound.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
The definition of a state in SO(2)
The definition of a state in SO(3) represented as a unit quaternion.
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
The definition of a time state.
A shared pointer wrapper for ompl::control::SpaceInformation.
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.
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.
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.
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.
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.
ProjectionPtr makeProjection(const base::SpaceInformationPtr &Bundle, const base::SpaceInformationPtr &Base)
Guess projection(s) between two SpaceInformationPtr Bundle and Base.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_DISCRETE
ompl::base::DiscreteStateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ STATE_SPACE_TIME
ompl::base::TimeStateSpace
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
Definition Cost.cpp:39
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 ...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()