Add waypoint to Edge

include/hpp/manipulation/graph/edge.hh

@@ -92,7 +92,11 @@ namespace hpp {
             isInNodeFrom_ = iinf;
           }
 
-          bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+          virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const;
+
+          virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
+
+          EdgePtr_t createWaypoint ();
 
         protected:
           /// Initialization of the object.
@@ -104,6 +108,7 @@ namespace hpp {
 
         private:
           typedef Cache < ConstraintSetPtr_t > Constraint_t;
+          typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint;
 
           /// The two ends of the transition.
           NodeWkPtr_t from_, to_;
@@ -111,6 +116,9 @@ namespace hpp {
           /// True if this path is in node from, False if in node to
           bool isInNodeFrom_;
 
+          Waypoint waypoint_;
+          mutable Configuration_t config_;
+
           /// See pathConstraint member function.
           Constraint_t* pathConstraints_;
 

src/graph/edge.cc

@@ -15,7 +15,9 @@
 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
 
 #include <hpp/core/straight-path.hh>
+#include <hpp/core/path-vector.hh>
 
+#include "hpp/manipulation/graph/statistics.hh"
 #include "hpp/manipulation/robot.hh"
 #include "hpp/manipulation/graph/edge.hh"
 
@@ -75,15 +77,67 @@ namespace hpp {
 
       bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
       {
-        ConstraintSetPtr_t constraints = pathConstraint ();
-        constraints->offsetFromConfig(q1);
-        if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
-          return false;
+        if (waypoint_.first) {
+          core::PathPtr_t pathToWaypoint;
+          config_ = q2;
+          if (!waypoint_.first->applyConstraints (q1, config_))
+            return false;
+          if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
+            return false;
+          core::PathVectorPtr_t pv = core::PathVector::create (graph_.lock ()->robot ()->configSize ());
+          path = pv;
+          pv->appendPath (pathToWaypoint);
+
+          ConstraintSetPtr_t constraints = pathConstraint ();
+          constraints->offsetFromConfig(config_);
+          if (!constraints->isSatisfied (config_) || !constraints->isSatisfied (q2)) {
+            return false;
+          }
+          core::PathPtr_t end = core::StraightPath::create (graph_.lock ()->robot (), config_, q2, d (config_, q2));
+          end->constraints (constraints);
+          pv->appendPath (end);
+        } else {
+          ConstraintSetPtr_t constraints = pathConstraint ();
+          constraints->offsetFromConfig(q1);
+          if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+            return false;
+          }
+          path = core::StraightPath::create (graph_.lock ()->robot (), q1, q2, d (q1, q2));
+          path->constraints (constraints);
         }
-        path = core::StraightPath::create (graph_.lock ()->robot (), q1, q2, d (q1, q2));
-        path->constraints (constraints);
         return true;
       }
+
+      bool Edge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
+      {
+        if (waypoint_.first) {
+          if (!waypoint_.first->applyConstraints (qoffset, q))
+            return false;
+        }
+        configConstraint ()->offsetFromConfig (qoffset);
+        if (configConstraint ()->apply (q))
+          return true;
+        typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
+        SuccessStatistics& ss = configConstraint ()->configProjector ()->statistics ();
+        if (ss.nbFailure () > ss.nbSuccess ()) {
+          hppDout (warning, configConstraint ()->name () << " fails often." << std::endl << ss);
+        } else {
+          hppDout (warning, configConstraint ()->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
+        }
+        return false;
+      }
+
+      EdgePtr_t Edge::createWaypoint ()
+      {
+        NodePtr_t node = Node::create ();
+        node->parentGraph(graph_);
+        EdgePtr_t edge = Edge::create (from (), node);
+        edge->parentGraph(graph_);
+        edge->isInNodeFrom (isInNodeFrom_);
+        waypoint_ = Waypoint (edge, node);
+        config_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
+        return edge;
+      }
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

src/manipulation-planner.cc

@@ -104,17 +104,9 @@ namespace hpp {
       // Select next node in the constraint graph.
       graph::NodePtr_t node = graph->getNode (*q_near);
       graph::EdgePtr_t edge = graph->chooseEdge (node);
-      ConstraintSetPtr_t constraint = graph->configConstraint (edge);
-      constraint->offsetFromConfig (*q_near);
       qProj_ = *q_rand;
-      if (!constraint->apply (qProj_)) {
+      if (!edge->applyConstraints (*q_near, qProj_)) {
         addFailure (PROJECTION, edge);
-        SuccessStatistics& ss = constraint->configProjector ()->statistics ();
-        if (ss.nbFailure () > ss.nbSuccess ()) {
-          hppDout (warning, constraint->name () << " fails often." << std::endl << ss);
-        } else {
-          hppDout (warning, constraint->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
-        }
         return false;
       }
       core::SteeringMethodPtr_t sm (problem().steeringMethod());