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{
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 {
109 qGoal_->isGoal = true;
110 }
111 getGoalPtr()->sampleGoal(qGoal_->state);
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}
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
Configuration * qGoal_
The (best) goal configuration.
Configuration * xRandom_
Temporary random configuration.
virtual const std::pair< Edge, bool > addEdge(const Vertex a, const Vertex b)
Add edge between Vertex a and Vertex b to graph.
virtual void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
bool findSection() override
Call algorithm to solve the find section problem.
virtual Vertex getGoalIndex() const
Get vertex representing the goal.
bool hasSolution_
If there exists a solution.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
bool firstRun_
Variable to check if this bundle space planner has been run at.
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