QMPImpl.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, 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, Sohaib Akbar */
37 
38 #ifndef OMPL_MULTILEVEL_PLANNERS_BundleSpace_QMPIMPL_
39 #define OMPL_MULTILEVEL_PLANNERS_BundleSpace_QMPIMPL_
40 #include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
41 #include <ompl/datastructures/PDF.h>
42 
43 namespace ompl
44 {
45  namespace base
46  {
47  OMPL_CLASS_FORWARD(OptimizationObjective);
48  }
49  namespace multilevel
50  {
51  OMPL_CLASS_FORWARD(PathGeometric);
53  class QMPImpl : public ompl::multilevel::BundleSpaceGraph
54  {
55  using BaseT = BundleSpaceGraph;
56 
57  public:
58  QMPImpl(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent_);
59  virtual ~QMPImpl() override;
60 
62  virtual void grow() override;
63  virtual void clear() override;
64 
66  void expand();
67 
68  virtual unsigned int computeK();
69 
70  void connectNeighbors(Configuration *x);
71 
72  virtual Vertex addConfiguration(Configuration *q) override;
73  void deleteConfiguration(Configuration *q) override;
74  void updatePDF(Configuration *q);
75 
76  protected:
77  std::vector<base::State *> randomWorkStates_;
78 
80  unsigned int k_NearestNeighbors_{10};
81 
82  PDF pdf;
83  };
84  } // namespace multilevel
85 } // namespace ompl
86 
87 #endif
void expand()
sample random node from Probabilty density function
Definition: QMPImpl.cpp:163
A shared pointer wrapper for ompl::base::SpaceInformation.
BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
Bundle Space contains three primary characters, the bundle space, the base space and the projection.
Definition: BundleSpace.cpp:60
virtual void grow() override
One iteration of QMP with adjusted sampling function.
Definition: QMPImpl.cpp:99
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: QMPImpl.cpp:66
unsigned int k_NearestNeighbors_
k nearest variable
Definition: QMPImpl.h:113
A graph on a Bundle-space.
virtual Vertex addConfiguration(Configuration *q) override
Add configuration to graph. Return its vertex in boost graph.
Definition: QMPImpl.cpp:72
Main namespace. Contains everything in this library.
Definition: AppBase.h:21