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 
43 namespace ompl
44 {
45  namespace geometric
46  {
48  class QuotientSpace : public ompl::base::Planner
49  {
50  using BaseT = ompl::base::Planner;
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);
86  ~QuotientSpace();
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 
108  const ompl::base::SpaceInformationPtr &getX1() const;
111  const ompl::base::SpaceInformationPtr &getQ1() const;
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 
176  const ompl::base::StateSpacePtr Q0);
177 
179  QuotientSpaceType identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1,
180  const ompl::base::StateSpacePtr Q0);
181 
185 
186  ompl::base::StateSamplerPtr X1_sampler_;
187  ompl::base::StateSamplerPtr Q1_sampler_;
188  ompl::base::ValidStateSamplerPtr Q1_valid_sampler_;
189 
191 
193  ompl::base::State *s_Q0_tmp_{nullptr};
195  ompl::base::State *s_X1_tmp_{nullptr};
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
Base class for a planner.
Definition: Planner.h:287
const ompl::base::StateSpacePtr computeQuotientSpace(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Compute the quotient Q1 / Q0 between two given spaces.
unsigned int getQ0Dimension() const
Dimension of space Q0.
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
void checkSpaceHasFiniteMeasure(const ompl::base::StateSpacePtr space) const
Check if quotient-space is unbounded.
void setChild(QuotientSpace *child_)
Set pointer to less simplified quotient-space.
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.
Definition of an abstract state.
Definition: State.h:114
unsigned int getLevel() const
Level in abstraction hierarchy of quotient-spaces.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
unsigned int id_
Identity of space (to keep track of number of quotient-spaces created)
A shared pointer wrapper for ompl::base::OptimizationObjective.
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.
const ompl::base::SpaceInformationPtr & getX1() const
Get SpaceInformationPtr for X1 (Note: X1 is the second component of Q1 = Q0 x X1)
unsigned int getTotalNumberOfFeasibleSamples() const
Number of feasible samples drawn on space Q1.
QuotientSpace * getChild() const
Child is a less simplified quotient-space (lower in abstraction hierarchy)
void setLevel(unsigned int)
Change abstraction level.
unsigned int getTotalNumberOfSamples() const
Number of samples drawn on space Q1.
static void resetCounter()
reset counter for number of levels
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
ompl::base::State * s_X1_tmp_
A temporary state on X1.
unsigned int getDimension() const
Dimension of space Q1.
const ompl::base::SpaceInformationPtr & getQ0() const
Get SpaceInformationPtr for Q0 (Note: Q0 is the first component of Q1 = Q0 x X1)
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 identifyQuotientSpaceType(const ompl::base::StateSpacePtr Q1, const ompl::base::StateSpacePtr Q0)
Identify the type of the quotient Q1 / Q0.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
A class to store the exit status of Planner::solve()
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()....
unsigned int getX1Dimension() const
Dimension of space X1.
A shared pointer wrapper for ompl::base::ProblemDefinition.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final
solve disabled (use MultiQuotient::solve) final prevents subclasses to override
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)
unsigned int getQ1Dimension() const
Dimension of space Q1.
const ompl::base::SpaceInformationPtr & getQ1() const
Get SpaceInformationPtr for Q1 (Note: Q1 is the product space Q1 = Q0 x X1)
A shared pointer wrapper for ompl::base::StateSpace.
unsigned int level_
Level in sequence of quotient-spaces.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::ValidStateSampler.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
A single quotient-space.
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
QuotientSpaceType getType() const
Type of quotient-space.
ompl::base::State * s_Q0_tmp_
A temporary state on Q0.
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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22