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>
47#define foreach BOOST_FOREACH
49ompl::multilevel::QMPImpl::QMPImpl(
const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_)
51 setName(
"QMPImpl" + std::to_string(id_));
53 setMetric(
"geodesic");
54 setGraphSampler(
"randomedge");
55 setImportance(
"exponential");
57 randomWorkStates_.resize(5);
58 getBundle()->allocStates(randomWorkStates_);
61ompl::multilevel::QMPImpl::~QMPImpl()
63 getBundle()->freeStates(randomWorkStates_);
74 Vertex m = BaseT::addConfiguration(q);
76 q->setPDFElement(q_element);
80void ompl::multilevel::QMPImpl::deleteConfiguration(
Configuration *q)
82 q->setPDFElement(
nullptr);
83 BaseT::deleteConfiguration(q);
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);
94unsigned int ompl::multilevel::QMPImpl::computeK()
96 return k_NearestNeighbors_;
106 if (qGoal_ ==
nullptr)
109 qGoal_->isGoal =
true;
111 getGoalPtr()->sampleGoal(qGoal_->state);
112 addConfiguration(qGoal_);
113 addGoalConfiguration(qGoal_);
119 if (!sampleBundleValid(xRandom_->state))
124 addConfiguration(xNew);
127 connectNeighbors(xNew);
133 if (sameComponent(vStart_, getGoalIndex()))
140void ompl::multilevel::QMPImpl::connectNeighbors(
Configuration *x)
142 std::vector<Configuration *> nearestNeighbors;
144 BaseT::nearestDatastructure_->nearestK(x, computeK(), nearestNeighbors);
146 for (
unsigned int k = 0; k < nearestNeighbors.size(); k++)
150 x->total_connection_attempts++;
151 xNear->total_connection_attempts++;
153 if (connect(xNear, x))
155 x->successful_connection_attempts++;
156 xNear->successful_connection_attempts++;
170 int s = getBundle()->randomBounceMotion(getBundleSamplerPtr(), q->state, randomWorkStates_.size(),
171 randomWorkStates_,
false);
177 addConfiguration(last);
178 connectNeighbors(last);
180 for (
int i = 0; i < s; i++)
183 addConfiguration(tmp);
A class that will hold data contained in the PDF.
A configuration in Bundle-space.
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...
virtual void grow() override
One iteration of QMP with adjusted sampling function.
virtual Vertex addConfiguration(Configuration *q) override
Add configuration to graph. Return its vertex in boost graph.
void expand()
sample random node from Probabilty density function