Loading...
Searching...
No Matches
MultiQuotientImpl.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#include <ompl/geometric/planners/quotientspace/datastructures/PlannerDataVertexAnnotated.h>
38#include <ompl/base/spaces/SO2StateSpace.h>
39#include <ompl/base/spaces/SO3StateSpace.h>
40#include <ompl/base/goals/GoalSampleableRegion.h>
41#include <ompl/base/goals/GoalState.h>
42#include <ompl/util/Exception.h>
43#include <ompl/util/Time.h>
44#include <queue>
45
46template <class T>
47ompl::geometric::MultiQuotient<T>::MultiQuotient(std::vector<ompl::base::SpaceInformationPtr> &siVec, std::string type)
48 : ompl::base::Planner(siVec.back(), type), siVec_(siVec)
49{
50 T::resetCounter();
51 for (unsigned int k = 0; k < siVec_.size(); k++)
52 {
53 QuotientSpace *parent = nullptr;
54 if (k > 0)
55 parent = quotientSpaces_.back();
56
57 T *ss = new T(siVec_.at(k), parent);
58 quotientSpaces_.push_back(ss);
59 quotientSpaces_.back()->setLevel(k);
60 }
62 OMPL_DEVMSG2("Created %d QuotientSpace levels.", siVec_.size());
63}
64
65template <class T>
67{
68 return stopAtLevel_;
69}
70
71template <class T>
73{
74 std::vector<int> nodesPerLevel;
75 for (unsigned int k = 0; k < stopAtLevel_; k++)
76 {
77 unsigned int Nk = quotientSpaces_.at(k)->getTotalNumberOfSamples();
78 nodesPerLevel.push_back(Nk);
79 }
80 return nodesPerLevel;
81}
82
83template <class T>
85{
86 std::vector<int> feasibleNodesPerLevel;
87 for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
88 {
89 unsigned int Nk = quotientSpaces_.at(k)->getTotalNumberOfFeasibleSamples();
90 feasibleNodesPerLevel.push_back(Nk);
91 }
92 return feasibleNodesPerLevel;
93}
94
95template <class T>
97{
98 std::vector<int> dimensionsPerLevel;
99 for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
100 {
101 unsigned int Nk = quotientSpaces_.at(k)->getDimension();
102 dimensionsPerLevel.push_back(Nk);
103 }
104 return dimensionsPerLevel;
105}
106
107template <class T>
109{
110}
111
112template <class T>
114{
115 BaseT::setup();
116 for (unsigned int k = 0; k < stopAtLevel_; k++)
117 {
118 quotientSpaces_.at(k)->setup();
119 }
120 currentQuotientLevel_ = 0;
121}
122
123template <class T>
125{
126 if (level_ > quotientSpaces_.size())
127 {
128 stopAtLevel_ = quotientSpaces_.size();
129 }
130 else
131 {
132 stopAtLevel_ = level_;
133 }
134}
135
136template <class T>
138{
139 Planner::clear();
140
141 for (unsigned int k = 0; k < quotientSpaces_.size(); k++)
142 {
143 quotientSpaces_.at(k)->clear();
144 }
145 currentQuotientLevel_ = 0;
146
147 while (!priorityQueue_.empty())
148 priorityQueue_.pop();
149 foundKLevelSolution_ = false;
150
151 solutions_.clear();
152 pdef_->clearSolutionPaths();
153}
154
155template <class T>
157{
159
160 for (unsigned int k = currentQuotientLevel_; k < stopAtLevel_; k++)
161 {
162 foundKLevelSolution_ = false;
163
164 if (priorityQueue_.size() <= currentQuotientLevel_)
165 priorityQueue_.push(quotientSpaces_.at(k));
166
168 [this, &ptc] { return ptc || foundKLevelSolution_; });
169
170 while (!ptcOrSolutionFound())
171 {
172 QuotientSpace *jQuotient = priorityQueue_.top();
173 priorityQueue_.pop();
174 jQuotient->grow();
175
176 bool hasSolution = quotientSpaces_.at(k)->hasSolution();
177 if (hasSolution)
178 {
180 quotientSpaces_.at(k)->getSolution(sol_k);
181 solutions_.push_back(sol_k);
182 double t_k_end = ompl::time::seconds(ompl::time::now() - t_start);
183 OMPL_DEBUG("Found Solution on Level %d after %f seconds.", k, t_k_end);
184 foundKLevelSolution_ = true;
185 currentQuotientLevel_ = k + 1;
186
187 // add solution to pdef
188 ompl::base::PlannerSolution psol(sol_k);
189 std::string lvl_name = getName() + " LvL" + std::to_string(k);
190 psol.setPlannerName(lvl_name);
191 quotientSpaces_.at(k)->getProblemDefinition()->addSolutionPath(psol);
192 }
193 priorityQueue_.push(jQuotient);
194 }
195
196 if (!foundKLevelSolution_)
197 {
198 OMPL_DEBUG("Planner failed finding solution on QuotientSpace level %d", k);
200 }
201 }
202 double t_end = ompl::time::seconds(ompl::time::now() - t_start);
203 OMPL_DEBUG("Found exact solution after %f seconds.", t_end);
204
206 if (quotientSpaces_.at(currentQuotientLevel_ - 1)->getSolution(sol))
207 {
209 psol.setPlannerName(getName());
210 pdef_->addSolutionPath(psol);
211 }
212
214}
215
216template <class T>
218ompl::geometric::MultiQuotient<T>::getProblemDefinition(unsigned int kQuotientSpace) const
219{
220 assert(kQuotientSpace >= 0);
221 assert(kQuotientSpace <= siVec_.size() - 1);
222 return quotientSpaces_.at(kQuotientSpace)->getProblemDefinition();
223}
224
225template <class T>
227{
228 this->Planner::setProblemDefinition(pdef);
229
230 // Compute projection of qInit and qGoal onto QuotientSpaces
231 ompl::base::Goal *goal = pdef_->getGoal().get();
232 ompl::base::GoalState *goalRegion = dynamic_cast<ompl::base::GoalState *>(goal);
233 double epsilon = goalRegion->getThreshold();
234 assert(quotientSpaces_.size() == siVec_.size());
235
236 ompl::base::State *sInit = pdef->getStartState(0);
237 ompl::base::State *sGoal = goalRegion->getState();
238
239 OMPL_DEVMSG1("Projecting start and goal onto QuotientSpaces.");
240
241 quotientSpaces_.back()->setProblemDefinition(pdef);
242
243 for (unsigned int k = siVec_.size() - 1; k > 0; k--)
244 {
245 QuotientSpace *quotientParent = quotientSpaces_.at(k);
246 QuotientSpace *quotientChild = quotientSpaces_.at(k - 1);
248 ompl::base::ProblemDefinitionPtr pdefk = std::make_shared<base::ProblemDefinition>(sik);
249
250 ompl::base::State *sInitK = sik->allocState();
251 ompl::base::State *sGoalK = sik->allocState();
252
253 quotientParent->projectQ0(sInit, sInitK);
254 quotientParent->projectQ0(sGoal, sGoalK);
255
256 pdefk->setStartAndGoalStates(sInitK, sGoalK, epsilon);
257
258 quotientChild->setProblemDefinition(pdefk);
259
260 sInit = sInitK;
261 sGoal = sGoalK;
262 }
263}
264
265template <class T>
267{
268 unsigned int Nvertices = data.numVertices();
269 if (Nvertices > 0)
270 {
271 OMPL_ERROR("PlannerData has %d vertices.", Nvertices);
272 throw ompl::Exception("cannot get planner data if plannerdata is already populated");
273 }
274
275 unsigned int K = std::min(solutions_.size() + 1, quotientSpaces_.size());
276 K = std::min(K, stopAtLevel_);
277
278 for (unsigned int k = 0; k < K; k++)
279 {
280 QuotientSpace *Qk = quotientSpaces_.at(k);
281 Qk->getPlannerData(data);
282
283 // label all new vertices
284 unsigned int ctr = 0;
285 for (unsigned int vidx = Nvertices; vidx < data.numVertices(); vidx++)
286 {
288 static_cast<ompl::base::PlannerDataVertexAnnotated &>(data.getVertex(vidx));
289 v.setLevel(k);
290 v.setMaxLevel(K);
291
292 ompl::base::State *s_lift = Qk->getSpaceInformation()->cloneState(v.getState());
293 v.setQuotientState(s_lift);
294
295 for (unsigned int m = k + 1; m < quotientSpaces_.size(); m++)
296 {
297 QuotientSpace *Qm = quotientSpaces_.at(m);
298
299 if (Qm->getX1() != nullptr)
300 {
301 ompl::base::State *s_X1 = Qm->getX1()->allocState();
302 ompl::base::State *s_Q1 = Qm->getSpaceInformation()->allocState();
303 if (Qm->getX1()->getStateSpace()->getType() == ompl::base::STATE_SPACE_SO3)
304 {
305 s_X1->as<ompl::base::SO3StateSpace::StateType>()->setIdentity();
306 }
307 if (Qm->getX1()->getStateSpace()->getType() == ompl::base::STATE_SPACE_SO2)
308 {
309 s_X1->as<ompl::base::SO2StateSpace::StateType>()->setIdentity();
310 }
311 Qm->mergeStates(s_lift, s_X1, s_Q1);
312 s_lift = Qm->getSpaceInformation()->cloneState(s_Q1);
313
314 Qm->getX1()->freeState(s_X1);
315 Qm->getQ1()->freeState(s_Q1);
316 }
317 }
318 v.setState(s_lift);
319 ctr++;
320 }
321 Nvertices = data.numVertices();
322 }
323}
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
Definition of a goal state.
Definition GoalState.h:49
const State * getState() const
Get the goal state.
Definition GoalState.cpp:79
Abstract definition of goals.
Definition Goal.h:63
A shared pointer wrapper for ompl::base::Path.
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
virtual const ompl::base::State * getState() const override
Retrieve the state associated with this vertex.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition Planner.cpp:66
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
A shared pointer wrapper for ompl::base::ProblemDefinition.
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::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
std::vector< int > getDimensionsPerLevel() const
Get all dimensions of the quotient-spaces in the sequence.
std::vector< int > getFeasibleNodes() const
Number of feasible nodes on each QuotientSpace (for DEBUGGING)
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
std::vector< ompl::base::SpaceInformationPtr > siVec_
Each QuotientSpace has a unique ompl::base::SpaceInformationPtr.
std::vector< QuotientSpace * > quotientSpaces_
Sequence of quotient-spaces.
unsigned int stopAtLevel_
Sometimes we only want to plan until a certain quotient-space level (for debugging for example)....
int getLevels() const
Number of quotient-spaces.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::vector< int > getNodes() const
Number of nodes on each QuotientSpace (for DEBUGGING)
MultiQuotient(std::vector< ompl::base::SpaceInformationPtr > &siVec, std::string type="QuotientPlanner")
Constructor taking a sequence of ompl::base::SpaceInformationPtr and computing the quotient-spaces fo...
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()....
void getPlannerData(ompl::base::PlannerData &data) const override
Return annotated vertices (with information about QuotientSpace level)
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...
A single quotient-space.
void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const
Merge a state from Q0 and X1 into a state on Q1 (concatenate)
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()....
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
void projectQ0(const ompl::base::State *q, ompl::base::State *qQ0) const
Quotient Space Projection Operator onto first component ProjectQ0: Q0 \times X1 \rightarrow Q0.
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
#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
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
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
Main namespace. Contains everything in this library.
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.