Loading...
Searching...
No Matches
QuotientSpace.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#ifndef OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_QUOTIENT_
39#define OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_QUOTIENT_
40
41#include <ompl/base/Planner.h>
42
43namespace ompl
44{
45 namespace geometric
46 {
49 {
51 enum QuotientSpaceType
52 {
53 UNKNOWN,
54 ATOMIC,
55 IDENTITY_SPACE_RN,
56 IDENTITY_SPACE_SE2,
57 IDENTITY_SPACE_SE2RN,
58 IDENTITY_SPACE_SO2RN,
59 IDENTITY_SPACE_SE3,
60 IDENTITY_SPACE_SE3RN,
61 RN_RM,
62 SE2_R2,
63 SE2RN_R2,
64 SE2RN_SE2,
65 SE2RN_SE2RM,
66 SO2RN_SO2,
67 SO2RN_SO2RM,
68 SE3_R3,
69 SE3RN_R3,
70 SE3RN_SE3,
71 SE3RN_SE3RM
72 };
73
74 public:
85 QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_ = nullptr);
87
91 virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override;
92
93 virtual void grow() = 0;
94 virtual bool getSolution(ompl::base::PathPtr &solution) = 0;
95 virtual bool sampleQuotient(ompl::base::State *q_random);
96 virtual bool sample(ompl::base::State *q_random);
97 virtual bool hasSolution();
98 virtual void clear() override;
99 virtual void setup() override;
100
101 virtual double getImportance() const;
102
104 static void resetCounter();
105
115
117 unsigned int getX1Dimension() const;
119 unsigned int getQ1Dimension() const;
121 unsigned int getQ0Dimension() const;
123 unsigned int getDimension() const;
124
125 const ompl::base::StateSamplerPtr &getX1SamplerPtr() const;
126 const ompl::base::StateSamplerPtr &getQ1SamplerPtr() const;
127
130 QuotientSpace *getParent() const;
133 QuotientSpace *getChild() const;
135 unsigned int getLevel() const;
137 void setLevel(unsigned int);
139 QuotientSpaceType getType() const;
141 void setChild(QuotientSpace *child_);
143 void setParent(QuotientSpace *parent_);
144
146 unsigned int getTotalNumberOfSamples() const;
148 unsigned int getTotalNumberOfFeasibleSamples() const;
149
152 void projectX1(const ompl::base::State *q, ompl::base::State *qX1) const;
155 void projectQ0(const ompl::base::State *q, ompl::base::State *qQ0) const;
157 void mergeStates(const ompl::base::State *qQ0, const ompl::base::State *qX1, ompl::base::State *qQ1) const;
158
161
162 ompl::base::OptimizationObjectivePtr getOptimizationObjectivePtr() const;
163
167 friend std::ostream &operator<<(std::ostream &out, const QuotientSpace &qtnt);
168
169 protected:
171 virtual void print(std::ostream &out) const;
172
177
179 QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1,
181
185
186 ompl::base::StateSamplerPtr X1_sampler_;
187 ompl::base::StateSamplerPtr Q1_sampler_;
188 ompl::base::ValidStateSamplerPtr Q1_valid_sampler_;
189
191
196
197 QuotientSpaceType type_;
198 unsigned int Q1_dimension_{0};
199 unsigned int Q0_dimension_{0};
200 unsigned int X1_dimension_{0};
201
202 static unsigned int counter_;
204 unsigned int id_{0};
206 unsigned int level_{0};
207
208 bool hasSolution_{false};
209 bool firstRun_{true};
210
211 QuotientSpace *parent_{nullptr};
212 QuotientSpace *child_{nullptr};
213
214 unsigned int totalNumberOfSamples_{0};
215 unsigned int totalNumberOfFeasibleSamples_{0};
216 };
217 } // namespace geometric
218} // namespace ompl
219#endif
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:223
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 quotient-space.
Definition: QuotientSpace.h:49
unsigned int getQ1Dimension() const
Dimension of space Q1.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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()....
static void resetCounter()
reset counter for number of levels
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
ompl::base::State * s_Q0_tmp_
A temporary state on Q0.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
unsigned int getX1Dimension() const
Dimension of space X1.
virtual void print(std::ostream &out) const
Internal function implementing actual printing to stream.
void setParent(QuotientSpace *parent_)
Set pointer to more simplified quotient-space.
void projectX1(const ompl::base::State *q, ompl::base::State *qX1) const
Quotient Space Projection Operator onto second component ProjectX1: Q0 \times X1 \rightarrow X1.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
void setLevel(unsigned int)
Change abstraction level.
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
unsigned int getQ0Dimension() const
Dimension of space Q0.
unsigned int getDimension() const
Dimension of space Q1.
ompl::base::State * s_X1_tmp_
A temporary state on X1.
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
unsigned int level_
Level in sequence of quotient-spaces.
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.
friend std::ostream & operator<<(std::ostream &out, const QuotientSpace &qtnt)
Write class to stream (use as std::cout << *this << std::endl) Actual implementation is in void print...
QuotientSpaceType getType() const
Type of quotient-space.
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49