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,...
Definition: PlannerData.h:175
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)
Definition: SO2StateSpace.h:68
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:91
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...
Definition: MultiQuotient.h:59
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.
Definition: QuotientSpace.h:49
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()
Definition: PlannerStatus.h:49
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62