QRRTImpl.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_QRRTIMPL_
39 #define OMPL_GEOMETRIC_PLANNERS_QUOTIENTSPACE_QRRTIMPL_
40 #include <ompl/geometric/planners/quotientspace/datastructures/QuotientSpaceGraph.h>
41 #include <ompl/datastructures/PDF.h>
42 
43 namespace ompl
44 {
45  namespace base
46  {
47  OMPL_CLASS_FORWARD(OptimizationObjective);
48  }
49  namespace geometric
50  {
52  class QRRTImpl : public ompl::geometric::QuotientSpaceGraph
53  {
54  using BaseT = QuotientSpaceGraph;
55 
56  public:
57  QRRTImpl(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_);
58  virtual ~QRRTImpl() override;
60  virtual void grow() override;
61  virtual bool getSolution(ompl::base::PathPtr &solution) override;
63  double getImportance() const override;
65  virtual bool sample(ompl::base::State *q_random) override;
68  virtual bool sampleQuotient(ompl::base::State *) override;
69 
70  virtual void setup() override;
71  virtual void clear() override;
72 
73  void setGoalBias(double goalBias);
74  double getGoalBias() const;
75  void setRange(double distance);
76  double getRange() const;
77 
78  protected:
80  Configuration *qRandom_{nullptr};
82  std::vector<Vertex> shortestPathVertices_;
83 
85  double maxDistance_{.0};
87  double goalBias_{.05};
88 
91  };
92  } // namespace geometric
93 } // namespace ompl
94 
95 #endif
double maxDistance_
Maximum distance of expanding the tree.
Definition: QRRTImpl.h:118
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::SpaceInformation.
double goalBias_
Goal bias similar to RRT.
Definition: QRRTImpl.h:120
Definition of an abstract state.
Definition: State.h:114
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QRRTImpl.cpp:84
ompl::base::Goal * goal_
Goal state or goal region.
Definition: QRRTImpl.h:123
Configuration * qRandom_
Random configuration placeholder.
Definition: QRRTImpl.h:113
virtual bool sample(ompl::base::State *q_random) override
Uniform sampling.
Definition: QRRTImpl.cpp:182
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: QRRTImpl.cpp:76
double getImportance() const override
Importance based on how many vertices the tree has.
Definition: QRRTImpl.cpp:162
A graph on a quotient-space.
Abstract definition of goals.
Definition: Goal.h:127
virtual void grow() override
One iteration of RRT with adjusted sampling function.
Definition: QRRTImpl.cpp:106
virtual bool sampleQuotient(ompl::base::State *) override
Quotient-Space sampling by choosing a random vertex from parent class tree.
Definition: QRRTImpl.cpp:204
QuotientSpace(const ompl::base::SpaceInformationPtr &si, QuotientSpace *parent_=nullptr)
Quotient Space contains three OMPL spaces, which we call Q1, Q0 and X1.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
std::vector< Vertex > shortestPathVertices_
Current shortest path on tree.
Definition: QRRTImpl.h:115