Loading...
Searching...
No Matches
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
47template <class T>
48ompl::multilevel::BundleSpaceSequence<T>::BundleSpaceSequence(ompl::base::SpaceInformationPtr si, std::string type)
49 : BaseT(si, type)
50{
51 declareBundleSpaces(true);
52}
53
54template <class T>
55ompl::multilevel::BundleSpaceSequence<T>::BundleSpaceSequence(std::vector<ompl::base::SpaceInformationPtr> &siVec,
56 std::string type)
57 : BaseT(siVec, type)
58{
59 declareBundleSpaces(true);
60}
61
62template <class T>
63ompl::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
84template <class T>
85void ompl::multilevel::BundleSpaceSequence<T>::declareBundleSpaces(bool guessProjection)
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();
100 if (guessProjection)
102 for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
104 bundleSpaces_.at(k)->makeProjection();
106 }
107
108 OMPL_DEBUG("Created %d BundleSpace levels (%s).", siVec_.size(), getName().c_str());
109}
110
111template <class T>
112ompl::multilevel::BundleSpaceSequence<T>::~BundleSpaceSequence()
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
124template <class T>
125void ompl::multilevel::BundleSpaceSequence<T>::setStopLevel(unsigned int level_)
126{
127 if (level_ > bundleSpaces_.size())
128 {
129 stopAtLevel_ = bundleSpaces_.size();
130 }
131 else
132 {
133 stopAtLevel_ = level_;
134 }
135}
136
137template <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
150template <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 }
159}
160
161template <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 }
171
172 while (!priorityQueue_.empty())
173 priorityQueue_.pop();
174
175 foundKLevelSolution_ = false;
176}
177
178template <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
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);
213 if (currentBundleSpaceLevel_ > (bundleSpaces_.size() - 1))
215 }
216 else
217 {
218 solutions_.at(k) = sol_k;
219 }
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
242 {
243 OMPL_DEBUG("-- Planner failed finding solution on BundleSpace level %d", k);
245 }
246 }
247
248 ompl::base::PathPtr sol;
250 static_cast<BundleSpace *>(bundleSpaces_.back())->getProblemDefinition()->getSolution(psol);
251 pdef_->addSolutionPath(psol);
252
254}
255
256template <class T>
257void ompl::multilevel::BundleSpaceSequence<T>::setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef)
258{
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();
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
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
346template <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
375template <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 {
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}
The exception type for ompl.
Definition Exception.h:47
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:82
Abstract definition of a goal region that can be sampled.
Definition of a goal state.
Definition GoalState.h:49
const State * getState() const
Get the goal state.
Definition GoalState.cpp:79
Definition of a set of goal states.
Definition GoalStates.h:50
virtual std::size_t getStateCount() const
Return the number of valid goal states.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition Planner.cpp:71
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
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
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
Definition of an abstract state.
Definition State.h:50
A graph on a Bundle-space.
void setFindSectionStrategy(FindSectionType type)
Set strategy to use to solve the find section problem.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
unsigned int currentBundleSpaceLevel_
Current level on which we have not yet found a path.
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()....
std::vector< T * > bundleSpaces_
Sequence of BundleSpaces.
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...
virtual void clear() override
Clear multilevel planner by clearing all levels.
bool foundKLevelSolution_
Indicator if a solution has been found on the current BundleSpaces.
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about BundleSpace level)
unsigned int stopAtLevel_
Sometimes we only want to plan until a certain BundleSpace level (for debugging for example)....
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...
BundleSpaceSequence(ompl::base::SpaceInformationPtr si, std::string type="BundleSpacePlannerNonMultilevel")
Non-multilevel Mode: Calling with a single ompl::base::SpaceInformationPtr will revert to standard pl...
A single Bundle-space.
Definition BundleSpace.h:64
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.
void setLevel(unsigned int)
Change level in hierarchy.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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.
virtual bool getSolution(ompl::base::PathPtr &solution)=0
Return best solution.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
ompl::base::State * allocIdentityStateBundle() const
Allocate State, set entries to Identity/Zero.
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....
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...
const ompl::base::State * getBaseState() const
Returns base state, indepent of mode.
void setMaxLevel(unsigned int level_)
The maximum level in the bundle space hierarchy.
void setLevel(unsigned int level_)
The level of vertex in the bundle space hierarchy.
std::vector< ompl::base::ProblemDefinitionPtr > pdefVec_
Sequence of ProblemDefinitionPtr.
std::vector< ompl::base::SpaceInformationPtr > siVec_
Each abstraction level has a unique ompl::base::SpaceInformationPtr.
virtual void clear() override
Clear multilevel planner by clearing all levels.
std::vector< ompl::base::PathPtr > solutions_
Solution paths on each abstraction level.
#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
GoalType
The type of goal.
Definition GoalTypes.h:46
@ GOAL_STATE
This bit is set if casting to goal state (ompl::base::GoalState) is possible.
Definition GoalTypes.h:59
@ GOAL_STATES
This bit is set if casting to goal states (ompl::base::GoalStates) is possible.
Definition GoalTypes.h:62
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition Time.h:52
point now()
Get the current time point.
Definition Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
Representation of a solution to a planning problem.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
A class to store the exit status of Planner::solve()
@ EXACT_SOLUTION
The planner found an exact solution.
@ TIMEOUT
The planner failed to find a solution.
@ INFEASIBLE
The planner decided that problem is infeasible.