Loading...
Searching...
No Matches
QMPImpl.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey, Sohaib Akbar */
38
39#include <ompl/multilevel/planners/qmp/QMPImpl.h>
40#include <ompl/tools/config/SelfConfig.h>
41#include <boost/foreach.hpp>
42#include <ompl/datastructures/NearestNeighbors.h>
43#include "ompl/datastructures/PDF.h"
44#include <ompl/geometric/PathGeometric.h>
45#include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
46
47#define foreach BOOST_FOREACH
48
49ompl::multilevel::QMPImpl::QMPImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
50{
51 setName("QMPImpl" + std::to_string(id_));
52
53 setMetric("geodesic");
54 setGraphSampler("randomedge");
55 setImportance("exponential");
56
57 randomWorkStates_.resize(5);
58 getBundle()->allocStates(randomWorkStates_);
59}
60
61ompl::multilevel::QMPImpl::~QMPImpl()
62{
63 getBundle()->freeStates(randomWorkStates_);
64}
65
67{
68 BaseT::clear();
69 pdf.clear();
70}
71
72ompl::multilevel::BundleSpaceGraph::Vertex ompl::multilevel::QMPImpl::addConfiguration(Configuration *q)
73{
74 Vertex m = BaseT::addConfiguration(q);
75 PDF_Element *q_element = pdf.add(q, 1.0);
76 q->setPDFElement(q_element);
77 return m;
78}
79
80void ompl::multilevel::QMPImpl::deleteConfiguration(Configuration *q)
81{
82 q->setPDFElement(nullptr);
83 BaseT::deleteConfiguration(q);
84}
85
86void ompl::multilevel::QMPImpl::updatePDF(Configuration *q)
87{
88 unsigned int N = q->total_connection_attempts;
89 unsigned int Ns = q->successful_connection_attempts;
90 double val = (double)(N - Ns) / (double)N;
91 pdf.update(static_cast<PDF_Element *>(q->getPDFElement()), val);
92}
93
94unsigned int ompl::multilevel::QMPImpl::computeK()
95{
96 return k_NearestNeighbors_;
97}
98
100{
101 if (firstRun_)
102 {
103 init();
104 firstRun_ = false;
105
106 if (qGoal_ == nullptr)
107 {
108 qGoal_ = new Configuration(getBundle());
109 qGoal_->isGoal = true;
110 }
111 getGoalPtr()->sampleGoal(qGoal_->state);
112 addConfiguration(qGoal_);
113 addGoalConfiguration(qGoal_);
114
115 findSection();
116 }
117
118 //(1) Get Random Sample
119 if (!sampleBundleValid(xRandom_->state))
120 return;
121
122 //(2) Add Configuration if valid
123 Configuration *xNew = new Configuration(getBundle(), xRandom_->state);
124 addConfiguration(xNew);
125
126 //(3) Connect to K nearest neighbors
127 connectNeighbors(xNew);
128
129 expand();
130
131 if (!hasSolution_)
132 {
133 if (sameComponent(vStart_, getGoalIndex()))
134 {
135 hasSolution_ = true;
136 }
137 }
138}
139
140void ompl::multilevel::QMPImpl::connectNeighbors(Configuration *x)
141{
142 std::vector<Configuration *> nearestNeighbors;
143
144 BaseT::nearestDatastructure_->nearestK(x, computeK(), nearestNeighbors);
145
146 for (unsigned int k = 0; k < nearestNeighbors.size(); k++)
147 {
148 Configuration *xNear = nearestNeighbors.at(k);
149
150 x->total_connection_attempts++;
151 xNear->total_connection_attempts++;
152
153 if (connect(xNear, x))
154 {
155 x->successful_connection_attempts++;
156 xNear->successful_connection_attempts++;
157 }
158 updatePDF(xNear);
159 }
160 updatePDF(x);
161}
162
164{
165 if (pdf.empty())
166 return;
167
168 Configuration *q = pdf.sample(rng_.uniform01());
169
170 int s = getBundle()->randomBounceMotion(getBundleSamplerPtr(), q->state, randomWorkStates_.size(),
171 randomWorkStates_, false);
172 if (s > 0)
173 {
174 Configuration *prev = q;
175
176 Configuration *last = new Configuration(getBundle(), randomWorkStates_[--s]);
177 addConfiguration(last);
178 connectNeighbors(last);
179
180 for (int i = 0; i < s; i++)
181 {
182 Configuration *tmp = new Configuration(getBundle(), randomWorkStates_[i]);
183 addConfiguration(tmp);
184
186 prev = tmp;
187 }
188 if (!sameComponent(prev->index, last->index))
190 }
191}
A class that will hold data contained in the PDF.
Definition PDF.h:53
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition QMPImpl.cpp:66
virtual void grow() override
One iteration of QMP with adjusted sampling function.
Definition QMPImpl.cpp:99
virtual Vertex addConfiguration(Configuration *q) override
Add configuration to graph. Return its vertex in boost graph.
Definition QMPImpl.cpp:72
void expand()
sample random node from Probabilty density function
Definition QMPImpl.cpp:163