ManipulationPlanner use pathProjector::Progressive instead of Dichotomy.

include/hpp/manipulation/manipulation-planner.hh

@@ -86,14 +86,15 @@ namespace hpp {
           PROJECTION,
           STEERING_METHOD,
           PATH_VALIDATION,
-          PATH_PROJECTION
+          PATH_PROJECTION_SHORTER,
+          PATH_PROJECTION_ZERO
         };
         struct Reasons {
           typedef ::hpp::statistics::SuccessBin::Reason Reason;
-          Reason projFailed, smFailed, pvFailed, pprojFailed;
+          Reason projFailed, smFailed, pvFailed, pprojShorter, pprojZero;
 
-          Reasons (const Reason& proj, const Reason& sm, const Reason& pv, const Reason& pp) :
-            projFailed (proj), smFailed (sm), pvFailed (pv), pprojFailed (pp) {}
+          Reasons (const Reason& proj, const Reason& sm, const Reason& pv, const Reason& pps, const Reason& ppz) :
+            projFailed (proj), smFailed (sm), pvFailed (pv), pprojShorter (pps), pprojZero (ppz) {}
           const Reason& get (TypeOfFailure t)
           {
             switch (t) {
@@ -103,8 +104,10 @@ namespace hpp {
                 return smFailed;
               case PATH_VALIDATION:
                 return pvFailed;
-              case PATH_PROJECTION:
-                return pprojFailed;
+              case PATH_PROJECTION_SHORTER:
+                return pprojShorter;
+              case PATH_PROJECTION_ZERO:
+                return pprojZero;
             }
             return ::hpp::statistics::SuccessBin::REASON_UNKNOWN;
           }

src/manipulation-planner.cc

@@ -23,6 +23,7 @@
 #include "hpp/manipulation/problem.hh"
 #include "hpp/manipulation/path-projector.hh"
 #include "hpp/manipulation/path-projector/dichotomy.hh"
+#include "hpp/manipulation/path-projector/progressive.hh"
 #include "hpp/manipulation/manipulation-planner.hh"
 #include "hpp/manipulation/graph/edge.hh"
 
@@ -52,7 +53,6 @@ namespace hpp {
       return false;
     }
 
-
     void ManipulationPlanner::oneStep ()
     {
       RobotPtr_t robot = HPP_DYNAMIC_PTR_CAST(Robot, problem ().robot ());
@@ -120,8 +120,11 @@ namespace hpp {
       }
       core::PathPtr_t projPath;
       if (!pathProjector_->apply (path, projPath)) {
-        addFailure (PATH_PROJECTION, edge);
-        if (!projPath || projPath->length () == 0) return false;
+        if (!projPath || projPath->length () == 0) {
+          addFailure (PATH_PROJECTION_ZERO, edge);
+          return false;
+        }
+        addFailure (PATH_PROJECTION_SHORTER, edge);
       }
       GraphPathValidationPtr_t pathValidation (problem_.pathValidation ());
       pathValidation->validate (projPath, false, validPath);
@@ -140,19 +143,19 @@ namespace hpp {
     {
       EdgeReasonMap::iterator it = failureReasons_.find (edge);
       if (it == failureReasons_.end ()) {
-        std::string edgeStr = "(" + edge->name () + ")";
-        Reasons r (SuccessBin::createReason ("Projection for " + edgeStr),
-                   SuccessBin::createReason ("SteeringMethod for " + edgeStr),
-                   SuccessBin::createReason ("PathValidation returned length 0 for " + edgeStr),
-                   SuccessBin::createReason ("Path could not be fully projected for " + edgeStr));
+        std::string edgeStr = edge->name () + " - ";
+        Reasons r (SuccessBin::createReason (edgeStr + "Projection"),
+                   SuccessBin::createReason (edgeStr + "SteeringMethod"),
+                   SuccessBin::createReason (edgeStr + "PathValidation returned length 0"),
+                   SuccessBin::createReason (edgeStr + "Path could not be fully projected"),
+                   SuccessBin::createReason (edgeStr + "Path could not be projected"));
         failureReasons_.insert (EdgeReasonPair (edge, r));
         extendStatistics_.addFailure (r.get (t));
         return;
       }
       Reasons r = it->second;
       extendStatistics_.addFailure (r.get (t));
-      hppDout (info, "Extension failed." << std::endl
-          << extendStatistics_);
+      hppDout (info, "Extension failed." << std::endl << extendStatistics_);
     }
 
     inline void ManipulationPlanner::tryConnect (const core::Nodes_t nodes)
@@ -195,7 +198,7 @@ namespace hpp {
         const core::RoadmapPtr_t& roadmap) :
       core::PathPlanner (problem, roadmap),
       shooter_ (new core::BasicConfigurationShooter (problem.robot ())),
-      problem_ (problem), pathProjector_ (new pathProjector::Dichotomy (problem_.distance (), 0.1)),
+      problem_ (problem), pathProjector_ (new pathProjector::Progressive (problem_.distance (), 0.2)),
       qProj_ (problem.robot ()->configSize ())
     {}