Benchmarks

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<base::State *> randomWorkStates_;

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 */

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<unsigned int>("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_))
             {

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_))
             {

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<double>(
+    /*Planner::declareParam<double>(
         "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_))
             {

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_))
             {

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};

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();
 

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)*/

src/ompl/geometric/planners/multilevel/datastructures/BundleSpaceSequenceImpl.h

@@ -319,3 +319,55 @@ void ompl::geometric::BundleSpaceSequence<T>::getPlannerData(ompl::base::Planner
     OMPL_DEBUG("Graph has %d/%d vertices/edges", 
         data.numVertices(), data.numEdges());
 }
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template <class T>
+void ompl::geometric::BundleSpaceSequence<T>::setMetric(const std::string& sMetric)
+{
+    for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
+    {
+        BundleSpace *qs = bundleSpaces_.at(k);
+        qs->setMetric(sMetric);
+    }
+}
+
+template <class T>
+void ompl::geometric::BundleSpaceSequence<T>::setImportance(const std::string& sImportance)
+{
+    for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
+    {
+        BundleSpace *qs = bundleSpaces_.at(k);
+        qs->setImportance(sImportance);
+    }
+}
+
+template <class T>
+void ompl::geometric::BundleSpaceSequence<T>::setGraphSampler(const std::string& sGraphSampler)
+{
+    for (unsigned int k = 0; k < bundleSpaces_.size(); k++)
+    {
+        BundleSpace *qs = bundleSpaces_.at(k);
+        qs->setGraphSampler(sGraphSampler);
+    }
+}
+
+template <class T>
+void ompl::geometric::BundleSpaceSequence<T>::setK(unsigned int k)
+{
+    for (unsigned int m = 0; m < bundleSpaces_.size(); m++)
+    {
+        BundleSpace *qs = bundleSpaces_.at(m);
+        qs->setK(k);
+    }
+}
+
+template <class T>
+void ompl::geometric::BundleSpaceSequence<T>::setFeasiblePathRestriction(bool val)
+{
+    for (unsigned int m = 0; m < bundleSpaces_.size(); m++)
+    {
+        BundleSpace *qs = bundleSpaces_.at(m);
+        qs->setFeasiblePathRestriction(val);
+    }
+}