Merge pull request ompl#4 from benjaminfresz/master

Merge

src/ompl/base/src/DynamicalMotionValidator.cpp

@@ -51,6 +51,8 @@ void ompl::base::DynamicalMotionValidator::defaultSettings()
       c_current = siC->allocControl();
       sampler = siC->allocDirectedControlSampler();
       s_target_copy = siC->allocState();
+    } else {
+      //si_->setStateValidityCheckingResolution(0.01*si_->getStateValidityCheckingResolution());
     }
 }
 
@@ -80,6 +82,7 @@ bool ompl::base::DynamicalMotionValidator::checkPath(const std::vector<ompl::bas
 bool ompl::base::DynamicalMotionValidator::checkMotion(const State *s1, const State *s2) const
 {
     /* assume motion starts in a valid configuration so s1 is valid */
+
     if (!si_->isValid(s2))
     {
         invalid_++;
@@ -88,6 +91,7 @@ bool ompl::base::DynamicalMotionValidator::checkMotion(const State *s1, const St
 
 
     if(!isDynamic){
+      //geometric case
       bool result = true;
       int nd = stateSpace_->validSegmentCount(s1, s2);
 

src/ompl/base/src/SpaceInformation.cpp

@@ -119,7 +119,7 @@ void ompl::base::SpaceInformation::setDefaultMotionValidator()
     //@TODO Was DiscreteMotionValidator, could not change to DynamicalMotionValidator
     // tried:
         motionValidator_ = std::make_shared<DynamicalMotionValidator>(this);
-        // motionValidator_ = std::make_shared<DiscreteMotionValidator>(this);
+        //motionValidator_ = std::make_shared<DiscreteMotionValidator>(this);
 }
 
 void ompl::base::SpaceInformation::setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)

src/ompl/control/SimpleDirectedControlSampler.h

@@ -63,7 +63,7 @@ namespace ompl
             /** \brief Constructor takes the state space to construct samples for as argument
                 Optionally, a \e k value can be given to indicate the number of controls to
                 try when directing a system toward a specific state.  Default value is 1. */
-            SimpleDirectedControlSampler(const SpaceInformation *si, unsigned int k = 200);
+            SimpleDirectedControlSampler(const SpaceInformation *si, unsigned int k = 500);
 
             ~SimpleDirectedControlSampler() override;
 
@@ -113,6 +113,11 @@ namespace ompl
 
             /** \brief The number of controls to sample when finding the best control*/
             unsigned int numControlSamples_;
+//##########################################
+	    double distanceFactor_{0.25};
+	    double toleratedDistance_;
+	    double dist;
+//##########################################
         };
     }
 }

src/ompl/control/src/SimpleDirectedControlSampler.cpp

@@ -68,17 +68,32 @@ unsigned int ompl::control::SimpleDirectedControlSampler::getBestControl(Control
     const unsigned int minDuration = si_->getMinControlDuration();
     const unsigned int maxDuration = si_->getMaxControlDuration();
 
+
+
     unsigned int steps = cs_->sampleStepCount(minDuration, maxDuration);
     // Propagate the first control, and find how far it is from the target state
     base::State *bestState = si_->allocState();
     steps = si_->propagateWhileValid(source, control, steps, bestState);
 
+//###############################################
+    //std::cout << minDuration << " to " << maxDuration << std::endl;
+    dist = si_->distance(source,dest);
+    //if((si_->getPropagationStepSize() * maxDuration) < (0.75*dist)){
+    //    std::cout << "too far away" << std::endl;
+    //    si_->copyState(dest, bestState);
+    //    si_->freeState(bestState);
+    //    return steps;
+    //}
+//###############################################
+
     if (numControlSamples_ > 1)
     {
         Control *tempControl = si_->allocControl();
         base::State *tempState = si_->allocState();
         double bestDistance = si_->distance(bestState, dest);
-
+//########################################
+	toleratedDistance_ = distanceFactor_ * dist;
+//#######################################
         // Sample k-1 more controls, and save the control that gets closest to target
         for (unsigned int i = 1; i < numControlSamples_; ++i)
         {
@@ -96,13 +111,23 @@ unsigned int ompl::control::SimpleDirectedControlSampler::getBestControl(Control
                 si_->copyControl(control, tempControl);
                 bestDistance = tempDistance;
                 steps = sampleSteps;
+//###########################################		
+		if(bestDistance < toleratedDistance_){
+		  si_->freeState(tempState);
+		  si_->freeControl(tempControl);
+		  std::cout << "Exited with " << i << " samples and " << steps << " steps" << std::endl;
+		  return steps;
+		}
+//##########################################
             }
         }
 
         si_->freeState(tempState);
         si_->freeControl(tempControl);
     }
 
+    std::cout << "checked Motion not feasible" << std::endl;
+
     si_->copyState(dest, bestState);
     si_->freeState(bestState);
 

src/ompl/geometric/planners/quotientspace/algorithms/QuotientTopology.h

@@ -50,7 +50,7 @@ namespace ompl
 
         int numberOfControlSamples{10};
         double propStepSize;
-        int controlDuration{10};
+        int controlDuration{200};
         double maxDistance{.0};
         double goalBias{.05};
         double epsilon{.0};

src/ompl/geometric/planners/quotientspace/algorithms/src/QuotientTopology.cpp

@@ -6,6 +6,7 @@
 #include <ompl/control/Control.h>
 #include <ompl/control/SimpleDirectedControlSampler.h>
 #include <ompl/control/StatePropagator.h>
+#include <ompl/base/DynamicalMotionValidator.h>
 
 using namespace og;
 using namespace ob;
@@ -20,15 +21,17 @@ QuotientTopology::QuotientTopology(const ob::SpaceInformationPtr &si, QuotientSp
 
   if(isDynamic) {
     ompl::control::SpaceInformation *siC = dynamic_cast<ompl::control::SpaceInformation*>(si.get());
+    siC->setMotionValidator(std::make_shared<ob::DynamicalMotionValidator>(siC));
+    siC->setup();
+    siC->setMinMaxControlDuration(1,controlDuration);
+    std::cout << "MaxControlDuration: " << controlDuration << std::endl;
 
     dCSampler = siC->allocDirectedControlSampler();
-    //only works for simpleDirectedControlSampler, which is used as default, but method can't be called
+    //@TODO: need to write allocator for simpleDirectedControlSampler
     //dCSampler->setNumControlSamples(numberOfControlSamples);
     propStepSize = siC->getPropagationStepSize();
     prop = siC->getStatePropagator();
     c_random = siC->allocControl();
-    //auto *rmotion = new Motion(siC);
-    //ompl::control::Control *c_random = rmotion->control;
   }
 
   q_random = new Configuration(Q1);
@@ -130,31 +133,29 @@ void QuotientTopology::growGeometric(){
 
 void QuotientTopology::growControl(){
   //do this, if control-case
-    //std::cout << "Got here 111111111111111111111111111111" << std::endl;
     const Configuration *q_nearest = nearest(q_random);
     s_random = q_random->state;
-    //std::cout << "Got here 222222222222222222222222222222" << std::endl;
-    //std::cout << c_random << std::endl;
-    //ompl::control::Control c_rand = ompl::control::Control();
+
     //changes q_random to the state we actually get with directed control c_random
     int duration = dCSampler->sampleTo(c_random, q_nearest->state, s_random);
     //c_random is always collisionfree if applied to q_nearest
-    //std::cout << "Got here 333333333333333333333333333333" << std::endl;
     totalNumberOfSamples_++;
     totalNumberOfFeasibleSamples_++;
+
     if(duration<controlDuration){
       //used control for full duration, add q_random
       Configuration *q_next = new Configuration(Q1, s_random);
       Vertex v_next = addConfiguration(q_next);
       addEdge(q_nearest->index, v_next);
-      //std::cout<<"1111111111"<<std::endl;
+      std::cout << "smaller" << std::endl;
+
     } else {
       //sets q_reached to the State we actually reach with our control for controlDuration
       prop->propagate(q_nearest->state, c_random, controlDuration,s_random);
       Configuration *q_next = new Configuration(Q1, s_random);
       Vertex v_next = addConfiguration(q_next);
       addEdge(q_nearest->index, v_next);
-      //std::cout<<"22222222222"<<std::endl;
+      std::cout << duration << std::endl;
     }
     if(!hasSolution_){
       bool satisfied = sameComponentSparse(v_start_sparse, v_goal_sparse);

src/ompl/geometric/planners/quotientspace/datastructures/PathVisibilityChecker.h

@@ -27,17 +27,19 @@ namespace ompl
       bool isPathClockwise(std::vector<ob::State*> &spath);
       bool CheckValidity(const std::vector<ob::State*> &s);
 
-      void createStateAt(ompl::control::SpaceInformation* si_,const std::vector<ob::State*> &path, const double &pathLength, const std::vector<double> &distances, const double newPosition, ob::State* s_interpolate) const;
-      void computePathLength(ompl::control::SpaceInformation* si_,const std::vector<ob::State*> &path, std::vector<double> &stateDistances, double &pathLength);
-      bool isStepDynamicallyFeasible(const ob::State* s_start, ob::State* s_target, const ompl::control::Control* c_previous, ompl::control::Control* c_current, const double targetRegion, const ompl::control::SpaceInformation* siC, const ompl::control::DirectedControlSamplerPtr sampler);
+      void createStateAt(ob::SpaceInformationPtr si_,const std::vector<ob::State*> &path, const double &pathLength, const std::vector<double> &distances, const double newPosition, ob::State* s_interpolate) const;
+      void computePathLength(ob::SpaceInformationPtr si_,const std::vector<ob::State*> &path, std::vector<double> &stateDistances, double &pathLength);
       bool isPathDynamicallyFeasible(const std::vector<ompl::base::State*> path) const;
 
+      void testCheckMotion(const ompl::base::State* s1, const ompl::base::State* s2);
+
       void Test1();
       void Test2();
       void Test3(int F=0);
 
     protected:
       bool isDynamic{false};
+
       ob::SpaceInformationPtr si_;
       ob::SpaceInformationPtr si_local;
       ob::State *lastValidState;
@@ -51,18 +53,15 @@ namespace ompl
 
       float max_planning_time_path_path{0.2};
       float epsilon_goalregion{0.05};
-
+
+      bool stepFeasible;
       std::vector<ob::State*> statesDyn;
       std::vector<ob::State*> statesDyn_next;
-      std::vector<ompl::control::Control*> controls;
-      std::vector<ompl::control::Control*> controls_next;
+
       int controlSamples{20};
       double pathSamples{10.};
       ompl::control::DirectedControlSamplerPtr sDCSampler;
-      double distanceThreshold{0.1};
-
-//      ompl::control::DirectedControlSamplerPtr simpleDirectedControlSamplerAllocator(const ompl::control::SpaceInformationPtr siC) const;
-
+      ompl::control::SpaceInformation* siC;
 
     private:
       std::vector<ob::State*> StatesFromVector(