[Refactoring] Update path planners to API change.

include/hpp/core/bi-rrt-planner.hh

@@ -49,7 +49,7 @@ namespace hpp {
       /// Store weak pointer to itself
       void init (const BiRRTPlannerWkPtr_t& weak);
       PathPtr_t extendInternal (const SteeringMethodPtr_t& sm, Configuration_t& qProj_, const NodePtr_t& near,
-                      const ConfigurationPtr_t& target, bool reverse=false);
+                      const Configuration_t& target, bool reverse=false);
 
       ConfigurationShooterPtr_t configurationShooter_;
       ConnectedComponentPtr_t startComponent_;

include/hpp/core/diffusing-planner.hh

@@ -50,7 +50,7 @@ namespace hpp {
       /// \param near node in the roadmap,
       /// \param target target configuration
       virtual PathPtr_t extend (const NodePtr_t& near,
-				const ConfigurationPtr_t& target);
+				const Configuration_t& target);
     private:
       ConfigurationShooterPtr_t configurationShooter_;
       mutable Configuration_t qProj_;

include/hpp/core/visibility-prm-planner.hh

@@ -58,13 +58,16 @@ namespace hpp {
 
       /// Return true if the configuration is visible from the given 
       /// connected component.
-      bool visibleFromCC (const ConfigurationPtr_t q, 
+      bool visibleFromCC (const Configuration_t q, 
 			  const ConnectedComponentPtr_t cc);
       
       /// Apply the problem constraints on a given configuration qTo by 
       /// projecting it on the tangent space of qFrom.
-      ConfigurationPtr_t applyConstraints (const ConfigurationPtr_t qFrom, 
-					   const ConfigurationPtr_t qTo);
+      void applyConstraints (
+          const Configuration_t& qFrom, 
+          const Configuration_t& qTo,
+          Configuration_t& qOut);
+
       bool constrApply_; // True if applyConstraints has successed
     };
     /// \}

src/bi-rrt-planner.cc

@@ -69,7 +69,7 @@ namespace hpp {
     }
 
     PathPtr_t BiRRTPlanner::extendInternal (const SteeringMethodPtr_t& sm, Configuration_t& qProj_, const NodePtr_t& near,
-                    const ConfigurationPtr_t& target, bool reverse)
+                    const Configuration_t& target, bool reverse)
     {
         const ConstraintSetPtr_t& constraints (sm->constraints ());
         if (constraints)
@@ -77,12 +77,12 @@ namespace hpp {
             ConfigProjectorPtr_t configProjector (constraints->configProjector ());
             if (configProjector)
             {
-                configProjector->projectOnKernel (*(near->configuration ()), *target,
+                configProjector->projectOnKernel (*(near->configuration ()), target,
                         qProj_);
             }
             else
             {
-                qProj_ = *target;
+                qProj_ = target;
             }
             if (constraints->apply (qProj_))
             {
@@ -93,7 +93,7 @@ namespace hpp {
                 return PathPtr_t ();
             }
         }
-        return reverse ? (*sm) (*target, *(near->configuration ())) : (*sm) (*(near->configuration ()), *target);
+        return reverse ? (*sm) (target, *(near->configuration ())) : (*sm) (*(near->configuration ()), target);
     }
 
 
@@ -118,7 +118,8 @@ namespace hpp {
         bool startComponentConnected(false), pathValidFromStart(false);
         ConfigurationPtr_t q_new;
         // first try to connect to start component
-        ConfigurationPtr_t q_rand = configurationShooter_->shoot ();
+        Configuration_t q_rand;
+        configurationShooter_->shoot (q_rand);
         near = roadmap()->nearestNode (q_rand, startComponent_, distance);
         path = extendInternal (problem().steeringMethod(), qProj_, near, q_rand);
         if (path)

src/diffusing-planner.cc

@@ -93,26 +93,26 @@ namespace hpp {
     }
 
     PathPtr_t DiffusingPlanner::extend (const NodePtr_t& near,
-					const ConfigurationPtr_t& target)
+					const Configuration_t& target)
     {
       const SteeringMethodPtr_t& sm (problem ().steeringMethod ());
       const ConstraintSetPtr_t& constraints (sm->constraints ());
       if (constraints) {
 	ConfigProjectorPtr_t configProjector (constraints->configProjector ());
 	if (configProjector) {
-	  configProjector->projectOnKernel (*(near->configuration ()), *target,
+	  configProjector->projectOnKernel (*(near->configuration ()), target,
 					    qProj_);
 	} else {
-	  qProj_ = *target;
+	  qProj_ = target;
 	}
 	if (!constraints->apply (qProj_)) {
 	  return PathPtr_t ();
 	}
       } else {
-        qProj_ = *target;
+        qProj_ = target;
       }
       // Here, qProj_ is a configuration that satisfies the constraints
-      // or *target if there are no constraints.
+      // or target if there are no constraints.
       PathPtr_t path = (*sm) (*(near->configuration ()), qProj_);
       PathProjectorPtr_t pp = problem ().pathProjector();
       if (pp) {
@@ -161,7 +161,8 @@ namespace hpp {
       Nodes_t newNodes, nearestNeighbors;
       PathPtr_t validPath, path;
       // Pick a random node
-      ConfigurationPtr_t q_rand = configurationShooter_->shoot ();
+      Configuration_t q_rand;
+      configurationShooter_->shoot (q_rand);
       //
       // First extend each connected component toward q_rand
       //

src/path-planner.cc

@@ -196,7 +196,7 @@ namespace hpp {
            itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
         if (*itCC != initCC) {
           value_type d;
-          NodePtr_t near (nn->search (initNode->configuration (),
+          NodePtr_t near (nn->search (*initNode->configuration (),
                                       *itCC, d, true));
           assert (near);
           ConfigurationPtr_t q1 (initNode->configuration ());
@@ -226,7 +226,7 @@ namespace hpp {
              itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
           if (*itCC != goalCC) {
             value_type d;
-            NodePtr_t near (nn->search ((*itn)->configuration (), *itCC, d,
+            NodePtr_t near (nn->search (*(*itn)->configuration (), *itCC, d,
                                         false));
             assert (near);
             ConfigurationPtr_t q1 (near->configuration ());

src/path-planner/k-prm-star.cc

@@ -94,7 +94,7 @@ namespace hpp {
       void kPrmStar::generateRandomConfig ()
       {
 	// shoot a valid random configuration
-	ConfigurationPtr_t qrand;
+	Configuration_t qrand;
 	// Report of configuration validation: unused here
 	ValidationReportPtr_t validationReport;
 	// Configuration validation methods associated to the problem
@@ -111,10 +111,10 @@ namespace hpp {
           bool valid (false);
           // After 10000 trials throw if no valid configuration has been found.
           do {
-            qrand = shooter->shoot ();
-            valid = (!constraints || constraints->apply (*qrand));
+            shooter->shoot (qrand);
+            valid = (!constraints || constraints->apply (qrand));
             if (valid)
-              valid = configValidations->validate (*qrand, validationReport);
+              valid = configValidations->validate (qrand, validationReport);
 	    nbTry++;
           } while (!valid && nbTry < 10000);
           if (!valid) {
@@ -126,7 +126,7 @@ namespace hpp {
           state_ = LINK_NODES;
           linkingNodeIt_ = r->nodes ().begin ();
           neighbors_ =roadmap ()->nearestNodes
-            ((*linkingNodeIt_)->configuration (), numberNeighbors_);
+            (*(*linkingNodeIt_)->configuration (), numberNeighbors_);
           itNeighbor_ = neighbors_.begin ();
         }
       }

src/visibility-prm-planner.cc

@@ -66,7 +66,7 @@ namespace hpp {
       weakPtr_ = weak;
     }
 
-    bool VisibilityPrmPlanner::visibleFromCC (const ConfigurationPtr_t q, 
+    bool VisibilityPrmPlanner::visibleFromCC (const Configuration_t q, 
 					      const ConnectedComponentPtr_t cc){
       PathPtr_t validPart;
       bool found = false; 
@@ -80,7 +80,7 @@ namespace hpp {
 	   n_it != cc->nodes ().end (); ++n_it){
 	if(nodeStatus_ [*n_it]){// only iterate on guard nodes
 	  ConfigurationPtr_t qCC = (*n_it)->configuration ();
-	  PathPtr_t path = (*sm) (*q, *qCC);
+	  PathPtr_t path = (*sm) (q, *qCC);
 	  PathValidationReportPtr_t report;
 	  if (path && pathValidation->validate (path, false, validPart,
 						report)){
@@ -88,7 +88,9 @@ namespace hpp {
 	    if (path->length () < length) {
 	      length = path->length ();
 	      // Save shortest edge
-	      delayedEdge = DelayedEdge_t (*n_it, q, path->reverse ());
+	      delayedEdge = DelayedEdge_t (*n_it,
+                  boost::make_shared<Configuration_t>(q),
+                  path->reverse ());
 	    }
 	    found = true;
 	  }
@@ -102,22 +104,22 @@ namespace hpp {
       else return false;
     }
 
-    ConfigurationPtr_t VisibilityPrmPlanner::applyConstraints 
-    (const ConfigurationPtr_t qFrom, const ConfigurationPtr_t qTo){
-      ConfigurationPtr_t qProj (new Configuration_t (*qTo));
+    void VisibilityPrmPlanner::applyConstraints (const Configuration_t& qFrom,
+        const Configuration_t& qTo, Configuration_t& qout)
+    {
       ConstraintSetPtr_t constraints (problem ().constraints ());
       if (constraints) {
 	ConfigProjectorPtr_t configProjector (constraints->configProjector ());
 	if (configProjector) {
-	  constrApply_ = false; // while apply has not successed
-	  configProjector->projectOnKernel (*qFrom, *qTo, *qProj);
-	  if (constraints->apply (*qProj)) {
+          constrApply_ = false; // while apply has not successed
+	  configProjector->projectOnKernel (qFrom, qTo, qout);
+	  if (constraints->apply (qout)) {
 	    constrApply_ = true;
-	    return qProj;
+	    return;
 	  }
 	}
       }
-      return qTo;
+      qout = qTo;
     }
 
     void VisibilityPrmPlanner::oneStep ()
@@ -126,11 +128,11 @@ namespace hpp {
       ConfigurationShooterPtr_t configurationShooter (problem().configurationShooter());
       ConfigValidationsPtr_t configValidations (problem ().configValidations());
       RoadmapPtr_t r (roadmap ());
-      ConfigurationPtr_t q_rand;
       value_type count; // number of times q has been seen
       constrApply_ = true; // stay true if no constraint in Problem
-      ConfigurationPtr_t q_init 
-	(new Configuration_t (*(r->initNode ()->configuration ())));
+      Configuration_t q_init (*(r->initNode ()->configuration ())),
+                      q_proj (robot->configSize()),
+                      q_rand (robot->configSize());
 
       /* Initialization of guard status */
       nodeStatus_ [r->initNode ()] = true; // init node is guard
@@ -142,11 +144,11 @@ namespace hpp {
       // Shoot random config as long as not collision-free
       ValidationReportPtr_t report;
       do {
-	q_rand = configurationShooter->shoot ();
-	q_rand = applyConstraints(q_init, q_rand);
-	robot->currentConfiguration (*q_rand);
+	configurationShooter->shoot (q_rand);
+	applyConstraints(q_init, q_rand, q_proj);
+	robot->currentConfiguration (q_proj);
 	robot->computeForwardKinematics ();
-      } while (!configValidations->validate (*q_rand, report) ||
+      } while (!configValidations->validate (q_proj, report) ||
 	       !constrApply_);
       count = 0;
 
@@ -154,16 +156,16 @@ namespace hpp {
 	     r->connectedComponents ().begin ();
 	   itcc != r->connectedComponents ().end (); ++itcc) {
 	ConnectedComponentPtr_t cc = *itcc;
-	if (visibleFromCC (q_rand, cc)) { 
+	if (visibleFromCC (q_proj, cc)) { 
 	  // delayedEdges_ will completed if visible
 	  count++; // count how many times q has been seen
 	}
       }
 	
       if (count == 0){ // q not visible from anywhere
-	NodePtr_t newNode = r->addNode (q_rand); // add q as a guard node
+	NodePtr_t newNode = r->addNode (q_proj); // add q as a guard node
 	nodeStatus_ [newNode] = true;
-	hppDout(info, "q is a guard node: " << displayConfig (*q_rand));
+	hppDout(info, "q is a guard node: " << displayConfig (q_proj));
       }
       if (count > 1){ // q visible several times
 	// Insert delayed edges from list and add q as a connection node