From 29102bd48f0a871063135ba3f9f68145aac4a4e1 Mon Sep 17 00:00:00 2001 From: SA Date: Sat, 20 Jun 2020 03:27:49 +0200 Subject: [PATCH] Benchmarks --- .../planners/multilevel/algorithms/QMPImpl.h | 2 + .../planners/multilevel/algorithms/SPQRImpl.h | 2 + .../multilevel/algorithms/src/QMPImpl.cpp | 16 +++++- .../multilevel/algorithms/src/QRRTImpl.cpp | 2 +- .../algorithms/src/QRRTStarImpl.cpp | 6 +-- .../multilevel/algorithms/src/SPQRImpl.cpp | 2 +- .../multilevel/datastructures/BundleSpace.h | 20 +++++++ .../datastructures/BundleSpaceGraph.h | 6 ++- .../datastructures/BundleSpaceSequence.h | 7 +++ .../datastructures/BundleSpaceSequenceImpl.h | 52 +++++++++++++++++++ 10 files changed, 107 insertions(+), 8 deletions(-) diff --git a/src/ompl/geometric/planners/multilevel/algorithms/QMPImpl.h b/src/ompl/geometric/planners/multilevel/algorithms/QMPImpl.h index 19e0d48f6a..2ed48a1afb 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/QMPImpl.h +++ b/src/ompl/geometric/planners/multilevel/algorithms/QMPImpl.h @@ -73,6 +73,8 @@ namespace ompl void deleteConfiguration(Configuration *q) override; void updatePDF(Configuration *q); + virtual void setK(unsigned int k) override; + unsigned int getK(); protected: std::vector randomWorkStates_; diff --git a/src/ompl/geometric/planners/multilevel/algorithms/SPQRImpl.h b/src/ompl/geometric/planners/multilevel/algorithms/SPQRImpl.h index 7ee00a04d4..acb8478ee0 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/SPQRImpl.h +++ b/src/ompl/geometric/planners/multilevel/algorithms/SPQRImpl.h @@ -69,6 +69,8 @@ namespace ompl virtual Vertex addConfiguration(Configuration *q) override; + void setK(unsigned int k) override {}; + protected: /** \brief Maximum failures limit for terminating the algorithm similar to SPARS */ diff --git a/src/ompl/geometric/planners/multilevel/algorithms/src/QMPImpl.cpp b/src/ompl/geometric/planners/multilevel/algorithms/src/QMPImpl.cpp index f209c380bf..733f63ffea 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/src/QMPImpl.cpp +++ b/src/ompl/geometric/planners/multilevel/algorithms/src/QMPImpl.cpp @@ -49,6 +49,7 @@ ompl::geometric::QMPImpl::QMPImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_) { setName("QMPImpl" + std::to_string(id_)); + //ompl::base::Planner::declareParam("knn", this, &ompl::geometric::QMPImpl::setK, &ompl::geometric::QMPImpl::getK); setMetric("geodesic"); setGraphSampler("randomedge"); @@ -69,6 +70,18 @@ void ompl::geometric::QMPImpl::clear() pdf.clear(); } +void ompl::geometric::QMPImpl::setK(unsigned int k) +{ + k_NearestNeighbors_ = k; + std::cout << "(" << name_ << ")" << "K is set to " << k_NearestNeighbors_ << std::endl; + OMPL_DEBUG("K is set to %i", k_NearestNeighbors_); +} + +unsigned int ompl::geometric::QMPImpl::getK() +{ + return k_NearestNeighbors_; +} + ompl::geometric::BundleSpaceGraph::Vertex ompl::geometric::QMPImpl::addConfiguration(Configuration *q) { @@ -95,6 +108,7 @@ void ompl::geometric::QMPImpl::updatePDF(Configuration *q) unsigned int ompl::geometric::QMPImpl::computeK() { + //std::cout << "K(" << name_ << ")=" << k_NearestNeighbors_ << std::endl; return k_NearestNeighbors_; } @@ -106,7 +120,7 @@ void ompl::geometric::QMPImpl::grow() vGoal_ = addConfiguration(qGoal_); firstRun_ = false; - if(hasBaseSpace()) + if(getFeasiblePathRestriction() && hasBaseSpace()) { if(getPathRestriction()->hasFeasibleSection(qStart_, qGoal_)) { diff --git a/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTImpl.cpp b/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTImpl.cpp index fbc3e98123..9c301f7121 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTImpl.cpp +++ b/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTImpl.cpp @@ -62,7 +62,7 @@ void ompl::geometric::QRRTImpl::grow() init(); firstRun_ = false; - if(hasBaseSpace()) + if(getFeasiblePathRestriction() && hasBaseSpace()) { if(getPathRestriction()->hasFeasibleSection(qStart_, qGoal_)) { diff --git a/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTStarImpl.cpp b/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTStarImpl.cpp index 153850ebaa..7f9e45dd2f 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTStarImpl.cpp +++ b/src/ompl/geometric/planners/multilevel/algorithms/src/QRRTStarImpl.cpp @@ -51,12 +51,12 @@ ompl::geometric::QRRTStarImpl::QRRTStarImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_) : BaseT(si, parent_) { setName("QRRTStarImpl" + std::to_string(id_)); - Planner::declareParam( + /*Planner::declareParam( "useKNearest_", this, &ompl::geometric::QRRTStarImpl::setKNearest, &ompl::geometric::QRRTStarImpl::getKNearest, - "0,1"); + "0,1");*/ symmetric_ = getBundle()->getStateSpace()->hasSymmetricInterpolate(); @@ -89,7 +89,7 @@ void ompl::geometric::QRRTStarImpl::grow() goal_ = pdef_->getGoal().get(); firstRun_ = false; - if(hasBaseSpace()) + if(getFeasiblePathRestriction() && hasBaseSpace()) { if(getPathRestriction()->hasFeasibleSection(qStart_, qGoal_)) { diff --git a/src/ompl/geometric/planners/multilevel/algorithms/src/SPQRImpl.cpp b/src/ompl/geometric/planners/multilevel/algorithms/src/SPQRImpl.cpp index 490ec863b1..9e7e749bb6 100644 --- a/src/ompl/geometric/planners/multilevel/algorithms/src/SPQRImpl.cpp +++ b/src/ompl/geometric/planners/multilevel/algorithms/src/SPQRImpl.cpp @@ -72,7 +72,7 @@ void ompl::geometric::SPQRImpl::grow() init(); firstRun_ = false; - if(hasBaseSpace()) + if(getFeasiblePathRestriction() && hasBaseSpace()) { if(getPathRestriction()->hasFeasibleSection(qStart_, qGoal_)) { diff --git a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpace.h b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpace.h index d24fb00e23..338d4f777b 100644 --- a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpace.h +++ b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpace.h @@ -153,6 +153,26 @@ namespace ompl bool isDynamic() const; + + // for benchmarks + //virtual void setMetric(const std::string& sMetric) = 0; + virtual void setImportance(const std::string& sImportance) = 0; + virtual void setGraphSampler(const std::string& sGraphSampler) = 0; + + + virtual void setK(unsigned int k) = 0; + + bool feasiblePathRestriction_{true}; + void setFeasiblePathRestriction(bool val) + { + feasiblePathRestriction_ = val; + } + bool getFeasiblePathRestriction() + { + return feasiblePathRestriction_; + } + + private: ompl::base::SpaceInformationPtr Bundle{nullptr}; diff --git a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceGraph.h b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceGraph.h index dfaf5d731a..0ea5903fa7 100644 --- a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceGraph.h +++ b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceGraph.h @@ -240,8 +240,10 @@ namespace ompl virtual void setMetric(const std::string& sMetric) override; virtual void setPropagator(const std::string& sPropagator) override; - virtual void setImportance(const std::string& sImportance); - virtual void setGraphSampler(const std::string& sGraphSampler); + virtual void setImportance(const std::string& sImportance) override; + virtual void setGraphSampler(const std::string& sGraphSampler) override; + + virtual void setK(unsigned int k) override {}; BundleSpaceGraphSamplerPtr getGraphSampler(); diff --git a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequence.h b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequence.h index e8b3a0c943..ebbd234cc0 100644 --- a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequence.h +++ b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequence.h @@ -89,6 +89,13 @@ namespace ompl void setStopLevel(unsigned int level_); + // for benchmarks + void setMetric(const std::string& sMetric); + void setImportance(const std::string& sImportance); + void setGraphSampler(const std::string& sGraphSampler); + void setK(unsigned int k); + void setFeasiblePathRestriction(bool val); + protected: /** \brief Solution paths on each BundleSpace (TODO: put into pdefs)*/ diff --git a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequenceImpl.h b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequenceImpl.h index b8332474e8..be3cfb8825 100644 --- a/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequenceImpl.h +++ b/src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequenceImpl.h @@ -319,3 +319,55 @@ void ompl::geometric::BundleSpaceSequence::getPlannerData(ompl::base::Planner OMPL_DEBUG("Graph has %d/%d vertices/edges", data.numVertices(), data.numEdges()); } + +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +template +void ompl::geometric::BundleSpaceSequence::setMetric(const std::string& sMetric) +{ + for (unsigned int k = 0; k < bundleSpaces_.size(); k++) + { + BundleSpace *qs = bundleSpaces_.at(k); + qs->setMetric(sMetric); + } +} + +template +void ompl::geometric::BundleSpaceSequence::setImportance(const std::string& sImportance) +{ + for (unsigned int k = 0; k < bundleSpaces_.size(); k++) + { + BundleSpace *qs = bundleSpaces_.at(k); + qs->setImportance(sImportance); + } +} + +template +void ompl::geometric::BundleSpaceSequence::setGraphSampler(const std::string& sGraphSampler) +{ + for (unsigned int k = 0; k < bundleSpaces_.size(); k++) + { + BundleSpace *qs = bundleSpaces_.at(k); + qs->setGraphSampler(sGraphSampler); + } +} + +template +void ompl::geometric::BundleSpaceSequence::setK(unsigned int k) +{ + for (unsigned int m = 0; m < bundleSpaces_.size(); m++) + { + BundleSpace *qs = bundleSpaces_.at(m); + qs->setK(k); + } +} + +template +void ompl::geometric::BundleSpaceSequence::setFeasiblePathRestriction(bool val) +{ + for (unsigned int m = 0; m < bundleSpaces_.size(); m++) + { + BundleSpace *qs = bundleSpaces_.at(m); + qs->setFeasiblePathRestriction(val); + } +} \ No newline at end of file