Graph can only have one NodeSelector.

include/hpp/manipulation/graph/graph.hh

@@ -42,45 +42,31 @@ namespace hpp {
           static GraphPtr_t create(RobotPtr_t robot);
 
           /// Create and insert a NodeSelector inside the graph.
-          NodeSelectorPtr_t createNodeSelector();
+          NodeSelectorPtr_t createNodeSelector ();
 
           /// Returns the states of a configuration.
-          Nodes_t getNode(const Configuration_t config) const;
+          NodePtr_t getNode (const Configuration_t config) const;
 
           /// Get possible edges between two nodes.
-          std::vector <Edges_t> getEdge(const Nodes_t& from, const Nodes_t& to) const;
+          Edges_t getEdges (const NodePtr_t& from, const NodePtr_t& to) const;
 
-          /// Select randomly outgoing edges of the given nodes.
-          Edges_t chooseEdge(const Nodes_t& node) const;
+          /// Select randomly outgoing edge of the given node.
+          EdgePtr_t chooseEdge(const NodePtr_t& node) const;
 
-          /// Constraint to project onto the Nodes_t.
-          /// \param the Nodes_t on which to project.
+          /// Constraint to project onto the Node.
+          /// \param the Node_t on which to project.
           /// \return The initialized projector.
-          ConstraintSetPtr_t configConstraint (const Nodes_t& nodes);
-
-          /// Kept for compatibility. Do not use. Use configConstraint(const Edges_t&) instead
-          /// \param config Configuration that will initialize the projector.
-          /// \return The initialized constraint.
-          ConstraintSetPtr_t configConstraint (const Edges_t& edges, ConfigurationIn_t config) __attribute__ ((deprecated));
+          ConstraintSetPtr_t configConstraint (const NodePtr_t& node);
 
           /// Constraint to project onto the same leaf as config.
           /// \param edges a list of edges defining the foliation.
           /// \return The constraint.
-          ConstraintSetPtr_t configConstraint (const Edges_t& edges);
+          ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge);
 
           /// Constraint to project a path.
-          /// \param edges a list of edges defining the foliation.
+          /// \param edge a list of edges defining the foliation.
           /// \return The constraint.
-          ConstraintSetPtr_t pathConstraint (const Edges_t& edges);
-
-          /// Kept for compatibility. Do not use. Use pathConstraint(const Edges_t&) instead
-          /// \param config Configuration that will initialize the constraint.
-          /// \return The initialized constraint.
-          ConstraintSetPtr_t pathConstraint (const Edges_t& edges, ConfigurationIn_t config) __attribute__ ((deprecated));
-
-          /// Return the NodeSelector with the given name if any,
-          /// NULL pointer if not found.
-          NodeSelectorPtr_t getNodeSelectorByName (const std::string& name);
+          ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge);
 
           /// Print the object in a stream.
           std::ostream& print (std::ostream& os) const;
@@ -110,7 +96,7 @@ namespace hpp {
 
         private:
           /// This list contains a node selector for each end-effector.
-          NodeSelectors_t nodeSelectors_;
+          NodeSelectorPtr_t nodeSelector_;
 
           /// A set of constraints that will always be used, for example
           /// stability constraints.
@@ -122,14 +108,14 @@ namespace hpp {
           /// Weak pointer to itself.
           GraphWkPtr_t wkPtr_;
 
-          /// Map of constraint sets (from Nodes).
-          typedef std::map  < Nodes_t, ConstraintSetPtr_t > MapFromNode;
-          typedef std::pair < Nodes_t, ConstraintSetPtr_t > PairNodesConstraints;
+          /// Map of constraint sets (from Node).
+          typedef std::map  < NodePtr_t, ConstraintSetPtr_t > MapFromNode;
+          typedef std::pair < NodePtr_t, ConstraintSetPtr_t > PairNodeConstraints;
           MapFromNode constraintSetMapFromNode_;
 
-          /// Map of constraint sets (from Edges).
-          typedef std::map  < Edges_t, ConstraintSetPtr_t > MapFromEdge;
-          typedef std::pair < Edges_t, ConstraintSetPtr_t > PairEdgesConstraints;
+          /// Map of constraint sets (from Edge).
+          typedef std::map  < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
+          typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
           MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
 
           value_type errorThreshold_;

include/hpp/manipulation/graph/statistics.hh

@@ -116,32 +116,23 @@ namespace hpp {
       {
         public :
           typedef ::hpp::statistics::Bin Parent;
-          NodeBin(const Nodes_t& ns): nodes_(ns), roadmapNodes_() {}
+          NodeBin(const NodePtr_t& n): node_(n), roadmapNodes_() {}
 
           void push_back(const core::NodePtr_t& n) {
             roadmapNodes_.push_back(n);
           }
 
           bool operator<(const NodeBin& rhs) const {
-            Nodes_t::const_iterator it1,
-              it2 = rhs.nodes ().begin();
-            for (it1 = nodes_.begin(); it1 != nodes_.end(); it1++) {
-              if ((*it1)->id () < (*it2)->id())
-                return true;
-              if ((*it1)->id () > (*it2)->id())
-                return false;
-              it2++;
-            }
-            return false;
+            return node_->id () < rhs.node ()->id ();
           }
 
           bool operator==(const NodeBin& rhs) const {
-            return nodes_ == rhs.nodes ();
+            return node_ == rhs.node ();
           }
 
-          const Nodes_t& nodes () const
+          const NodePtr_t& node () const
           {
-            return nodes_;
+            return node_;
           }
 
           std::ostream& print (std::ostream& os) const
@@ -173,18 +164,14 @@ namespace hpp {
           }
 
         private:
-          Nodes_t nodes_;
+          NodePtr_t node_;
 
           typedef std::list <core::NodePtr_t> RoadmapNodes_t;
           RoadmapNodes_t roadmapNodes_;
 
           std::ostream& printValue (std::ostream& os) const
           {
-            os << "NodeBin (";
-            Nodes_t::const_iterator it1;
-            for (it1 = nodes_.begin(); it1 != nodes_.end(); it1++)
-              os << (*it1)->name () << " / ";
-            return os << ")";
+            return os << "NodeBin (" << node_->name () << ")";
           }
       };
 

include/hpp/manipulation/manipulation-planner.hh

@@ -81,7 +81,7 @@ namespace hpp {
         typedef ::hpp::statistics::SuccessBin SuccessBin;
         SuccessStatistics extendStatistics_;
 
-        /// A Reason is associated to each Edges_t that generated a failure.
+        /// A Reason is associated to each EdgePtr_t that generated a failure.
         enum TypeOfFailure {
           PROJECTION,
           STEERING_METHOD,
@@ -106,11 +106,11 @@ namespace hpp {
             return ::hpp::statistics::SuccessBin::REASON_UNKNOWN;
           }
         };
-        typedef std::pair < graph::Edges_t, Reasons > EdgesReasonPair;
-        typedef std::map  < graph::Edges_t, Reasons > EdgesReasonMap;
-        EdgesReasonMap failureReasons_;
+        typedef std::pair < graph::EdgePtr_t, Reasons > EdgeReasonPair;
+        typedef std::map  < graph::EdgePtr_t, Reasons > EdgeReasonMap;
+        EdgeReasonMap failureReasons_;
 
-        void addFailure (TypeOfFailure t, const graph::Edges_t& edges);
+        void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
 
         mutable Configuration_t qProj_;
     };

src/graph-path-validation.cc

@@ -91,13 +91,13 @@ namespace hpp {
                  newTmax = pathNoCollision->timeRange ().second,
                  oldTmin = path->timeRange ().first,
                  oldTmax = path->timeRange ().second;
-      graph::Nodes_t origNodes, destNodes;
+      graph::NodePtr_t origNode, destNode;
       try {
-        origNodes = constraintGraph_->getNode (newPath (newTmin));
-        destNodes = constraintGraph_->getNode (newPath (newTmax));
+        origNode = constraintGraph_->getNode (newPath (newTmin));
+        destNode = constraintGraph_->getNode (newPath (newTmax));
 
-        if (origNodes == constraintGraph_->getNode ((*path) (oldTmin))
-            && destNodes == constraintGraph_->getNode ((*path) (oldTmax))) {
+        if (origNode == constraintGraph_->getNode ((*path) (oldTmin))
+            && destNode == constraintGraph_->getNode ((*path) (oldTmax))) {
           validPart = pathNoCollision;
           return false;
         }
@@ -113,7 +113,7 @@ namespace hpp {
       // - Use the steering method to create a new path and validate it.
       // - Return a null path.
       assert (!reverse && "This has never been tested with reverse path");
-      std::vector <graph::Edges_t> possibleEdges (constraintGraph_->getEdge (origNodes, destNodes));
+      graph::Edges_t possibleEdges (constraintGraph_->getEdges (origNode, destNode));
       // We check for all of them if both nodes are on the same leaf.
       ConstraintSetPtr_t constraints;
       while (!possibleEdges.empty ()) {

src/graph-steering-method.cc

@@ -31,9 +31,9 @@ namespace hpp {
     PathPtr_t GraphSteeringMethod::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
     {
       value_type length = (*distance_) (q1,q2);
-      std::vector< graph::Edges_t > possibleEdges;
+      graph::Edges_t possibleEdges;
       try {
-        possibleEdges = graph_->getEdge (graph_->getNode (q1), graph_->getNode (q2));
+        possibleEdges = graph_->getEdges (graph_->getNode (q1), graph_->getNode (q2));
       } catch (const std::logic_error& e) {
         hppDout (error, e.what ());
         return PathPtr_t ();

src/graph/edge.cc

@@ -47,8 +47,7 @@ namespace hpp {
       ConstraintPtr_t Edge::configConstraint(ConfigurationIn_t config)
       {
         if (!configConstraints_) {
-          Edges_t thisEdge; thisEdge.push_back (wkPtr_.lock ());
-          configConstraints_ = graph_.lock ()->configConstraint (thisEdge);
+          configConstraints_ = graph_.lock ()->configConstraint (wkPtr_.lock ());
         }
         configConstraints_->offsetFromConfig (config);
         return configConstraints_;
@@ -57,8 +56,7 @@ namespace hpp {
       ConstraintPtr_t Edge::pathConstraint(ConfigurationIn_t config)
       {
         if (!pathConstraints_) {
-          Edges_t thisEdge; thisEdge.push_back (wkPtr_.lock ());
-          pathConstraints_ = graph_.lock ()->pathConstraint (thisEdge);
+          pathConstraints_ = graph_.lock ()->pathConstraint (wkPtr_.lock ());
         }
         pathConstraints_->offsetFromConfig (config);
         return pathConstraints_;

src/graph/graph.cc

@@ -25,21 +25,13 @@
 namespace hpp {
   namespace manipulation {
     namespace graph {
-      static std::string toString (const Nodes_t& n) {
-        std::string nodesStr = "(";
-        for (graph::Nodes_t::const_iterator itNode = n.begin();
-            itNode != n.end (); itNode++)
-          nodesStr += (*itNode)->name () + " / ";
-        nodesStr += ")";
+      static std::string toString (const NodePtr_t& n) {
+        std::string nodesStr = "(" + n->name () + ")";
         return nodesStr;
       }
 
-      static std::string toString (const Edges_t& e) {
-        std::string edgesStr = "(";
-        for (graph::Edges_t::const_iterator itEdge = e.begin();
-            itEdge != e.end (); itEdge++)
-          edgesStr += (*itEdge)->name () + " / ";
-        edgesStr += ")";
+      static std::string toString (const EdgePtr_t& e) {
+        std::string edgesStr = "(" + e->name () + ")";
         return edgesStr;
       }
 
@@ -60,10 +52,9 @@ namespace hpp {
 
       NodeSelectorPtr_t Graph::createNodeSelector()
       {
-        NodeSelectorPtr_t newNodeSelector = NodeSelector::create();
-        nodeSelectors_.push_back(newNodeSelector);
-        newNodeSelector->parentGraph (wkPtr_);
-        return newNodeSelector;
+        nodeSelector_ = NodeSelector::create();
+        nodeSelector_->parentGraph (wkPtr_);
+        return nodeSelector_;
       }
 
       void Graph::maxIterations (size_type iterations)
@@ -91,158 +82,67 @@ namespace hpp {
         return robot_;
       }
 
-      Nodes_t Graph::getNode(const Configuration_t config) const
+      NodePtr_t Graph::getNode (const Configuration_t config) const
       {
-        Nodes_t nodes;
-        for (NodeSelectors_t::const_iterator it = nodeSelectors_.begin();
-            it != nodeSelectors_.end(); it++)
-          nodes.push_back( (*it)->getNode(config) );
-        return nodes;
+        return nodeSelector_->getNode (config);
       }
 
-      static void buildPossibleEdges (Edges_t current,
-          const std::vector <Edges_t>& edgesPerNodeSelector,
-          std::vector <Edges_t>& possibleEdges)
-      {
-        size_t d = current.size();
-        if (d == edgesPerNodeSelector.size()) {
-          possibleEdges.push_back (current);
-          return;
-        }
-        Edges_t::const_iterator it = edgesPerNodeSelector[d].begin();
-        while (it != edgesPerNodeSelector[d].end()) {
-          current.push_back (*it);
-          buildPossibleEdges (current, edgesPerNodeSelector, possibleEdges);
-          current.pop_back ();
-          it++;
-        }
-      }
-
-      std::vector <Edges_t> Graph::getEdge(const Nodes_t& from, const Nodes_t& to) const
-      {
-        assert (from.size() == to.size());
-        assert (nodeSelectors_.size() == to.size());
-        size_t numberPossibleEdges = 1;
-        std::vector < Edges_t > edgesPerNodeSelector (nodeSelectors_.size());
-        std::vector < Edges_t >::iterator itEdgePerNodeSelector = edgesPerNodeSelector.begin();
-        Nodes_t::const_iterator itFrom = from.begin (),
-                                itTo   =   to.begin ();
-        // We first iterate through from. For each element of from,
-        // we look for all edges between this element of from and its corresponding
-        // element in to. The resulting set of Edges_t is stored in
-        // edgesPerNodeSelector.
-
-        // Temporary variable.
-        Edges_t edgesInNodeSelector;
-        const Neighbors_t* neighbors;
-        Neighbors_t::const_iterator itEdge;
-        while (itFrom != from.end()) {
-          edgesInNodeSelector.clear ();
-          neighbors = &((*itFrom)->neighbors ());
-          itEdge = neighbors->begin();
-          // Find the edges between *itFrom and *itTo
-          while (itEdge != neighbors->end()) {
-            if (itEdge->second->to() == (*itTo) )
-              edgesInNodeSelector.push_back (itEdge->second);
-            itEdge++;
-          }
-          /// If no Edge is found, the two Node are not connected.
-          if (edgesInNodeSelector.empty ())
-            return std::vector <Edges_t>(0);
-
-          // Store the Edges.
-          numberPossibleEdges *= edgesInNodeSelector.size();
-          *itEdgePerNodeSelector = edgesInNodeSelector;
-
-          itFrom++; itTo++; itEdgePerNodeSelector++;
-        }
-        assert (itTo == to.end());
-        assert (itEdgePerNodeSelector == edgesPerNodeSelector.end());
-
-        // Now, we can create the list of possible Edges_t
-        // between from and to.
-        std::vector <Edges_t> possibleEdges;
-        buildPossibleEdges (Edges_t(), edgesPerNodeSelector, possibleEdges);
-        assert (possibleEdges.size() == numberPossibleEdges);
-        return possibleEdges;
-      }
-
-      Edges_t Graph::chooseEdge(const Nodes_t& nodes) const
+      Edges_t Graph::getEdges (const NodePtr_t& from, const NodePtr_t& to) const
       {
         Edges_t edges;
-        for (Nodes_t::const_iterator it = nodes.begin();
-            it != nodes.end(); it++)
-          edges.push_back( (*it)->nodeSelector().lock()->chooseEdge(*it) );
+        for (Neighbors_t::const_iterator it = from->neighbors ().begin ();
+            it != from->neighbors ().end (); it++) {
+          if (it->second->to () == to)
+            edges.push_back (it->second);
+        }
         return edges;
       }
 
-      NodeSelectorPtr_t Graph::getNodeSelectorByName (const std::string& name)
+      EdgePtr_t Graph::chooseEdge (const NodePtr_t& node) const
       {
-        for (NodeSelectors_t::iterator it = nodeSelectors_.begin();
-            it != nodeSelectors_.end(); it++) {
-          if (name == (*it)->name())
-            return *it;
-        }
-        return NodeSelectorPtr_t();
+        return nodeSelector_->chooseEdge (node);
       }
 
-      ConstraintSetPtr_t Graph::configConstraint (const Nodes_t& nodes)
+      ConstraintSetPtr_t Graph::configConstraint (const NodePtr_t& node)
       {
         ConstraintSetPtr_t constraint;
-        MapFromNode::const_iterator it = constraintSetMapFromNode_.find (nodes);
+        MapFromNode::const_iterator it = constraintSetMapFromNode_.find (node);
         if (it == constraintSetMapFromNode_.end ()) {
-          std::string name = toString (nodes);
+          std::string name = toString (node);
           constraint = ConstraintSet::create (robot (), "Set " + name);
 
           ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
           insertNumericalConstraints (proj);
-          for (Nodes_t::const_iterator it = nodes.begin();
-              it != nodes.end(); it++)
-            (*it)->insertNumericalConstraints (proj);
+          node->insertNumericalConstraints (proj);
           constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
 
           insertLockedDofs (constraint);
-          for (Nodes_t::const_iterator it = nodes.begin();
-              it != nodes.end(); it++)
-            (*it)->insertLockedDofs (constraint);
-          constraintSetMapFromNode_.insert (PairNodesConstraints(nodes, constraint));
+          node->insertLockedDofs (constraint);
+          constraintSetMapFromNode_.insert (PairNodeConstraints(node, constraint));
         } else {
           constraint = it->second;
         }
         return constraint;
       }
 
-      ConstraintSetPtr_t Graph::configConstraint (const Edges_t& edges, ConfigurationIn_t config)
-      {
-        ConstraintSetPtr_t constraint = configConstraint (edges);
-        constraint->offsetFromConfig (config);
-        return constraint;
-      }
-
-      ConstraintSetPtr_t Graph::configConstraint (const Edges_t& edges)
+      ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
       {
         ConstraintSetPtr_t constraint;
-        MapFromEdge::const_iterator it = cfgConstraintSetMapFromEdge_.find (edges);
+        MapFromEdge::const_iterator it = cfgConstraintSetMapFromEdge_.find (edge);
         if (it == cfgConstraintSetMapFromEdge_.end ()) {
-          std::string name = toString (edges);
+          std::string name = toString (edge);
           constraint = ConstraintSet::create (robot (), "Set " + name);
 
           ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
           insertNumericalConstraints (proj);
-          for (Edges_t::const_iterator it = edges.begin();
-              it != edges.end(); it++) {
-            (*it)->insertNumericalConstraints (proj);
-            (*it)->to ()->insertNumericalConstraints (proj);
-          }
+          edge->insertNumericalConstraints (proj);
+          edge->to ()->insertNumericalConstraints (proj);
           constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
 
           insertLockedDofs (constraint);
-          for (Edges_t::const_iterator it = edges.begin();
-              it != edges.end(); it++) {
-            (*it)->insertLockedDofs (constraint);
-            (*it)->to ()->insertLockedDofs (constraint);
-          }
-          cfgConstraintSetMapFromEdge_.insert (PairEdgesConstraints(edges, constraint));
+          edge->insertLockedDofs (constraint);
+          edge->to ()->insertLockedDofs (constraint);
+          cfgConstraintSetMapFromEdge_.insert (PairEdgeConstraints(edge, constraint));
         } else {
           constraint = it->second;
         }
@@ -250,37 +150,24 @@ namespace hpp {
         return constraint;
       }
 
-      ConstraintSetPtr_t Graph::pathConstraint (const Edges_t& edges, ConfigurationIn_t config)
-      {
-        ConstraintSetPtr_t constraint = pathConstraint (edges);
-        constraint->offsetFromConfig (config);
-        return constraint;
-      }
-
-      ConstraintSetPtr_t Graph::pathConstraint (const Edges_t& edges)
+      ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge)
       {
         ConstraintSetPtr_t constraint;
-        MapFromEdge::const_iterator it = pathConstraintSetMapFromEdge_.find (edges);
+        MapFromEdge::const_iterator it = pathConstraintSetMapFromEdge_.find (edge);
         if (it == pathConstraintSetMapFromEdge_.end ()) {
-          std::string name = toString (edges);
+          std::string name = toString (edge);
           constraint = ConstraintSet::create (robot (), "Set " + name);
 
           ConfigProjectorPtr_t proj = ConfigProjector::create(robot(), "proj " + name, errorThreshold(), maxIterations());
           insertNumericalConstraints (proj);
-          for (Edges_t::const_iterator it = edges.begin();
-              it != edges.end(); it++) {
-            (*it)->insertNumericalConstraints (proj);
-            (*it)->node ()->insertNumericalConstraintsForPath (proj);
-          }
+          edge->insertNumericalConstraints (proj);
+          edge->node ()->insertNumericalConstraintsForPath (proj);
           constraint->addConstraint (HPP_DYNAMIC_PTR_CAST(Constraint, proj));
 
           insertLockedDofs (constraint);
-          for (Edges_t::const_iterator it = edges.begin();
-              it != edges.end(); it++) {
-            (*it)->insertLockedDofs (constraint);
-            (*it)->node ()->insertLockedDofs (constraint);
-          }
-          pathConstraintSetMapFromEdge_.insert (PairEdgesConstraints (edges, constraint));
+          edge->insertLockedDofs (constraint);
+          edge->node ()->insertLockedDofs (constraint);
+          pathConstraintSetMapFromEdge_.insert (PairEdgeConstraints (edge, constraint));
         } else {
           constraint = it->second;
         }
@@ -290,11 +177,7 @@ namespace hpp {
 
       std::ostream& Graph::print (std::ostream& os) const
       {
-        GraphComponent::print (os) << std::endl;
-        for (NodeSelectors_t::const_iterator it = nodeSelectors_.begin();
-            it != nodeSelectors_.end(); it++)
-          os << *(*it);
-        return os;
+        return GraphComponent::print (os) << std::endl << nodeSelector_;
       }
     } // namespace graph
   } // namespace manipulation

src/graph/node.cc

@@ -47,9 +47,6 @@ namespace hpp {
 
       bool Node::contains (const Configuration_t config)
       {
-        // TODO: This is not the most efficient way. We should
-        // compute the value of the constraint instead of apllying
-        // the constraint.
         return configConstraint()->isSatisfied (config);
       }
 
@@ -65,8 +62,7 @@ namespace hpp {
       ConstraintPtr_t Node::configConstraint()
       {
         if (!configConstraints_) {
-          Nodes_t thisNode; thisNode.push_back (wkPtr_.lock ());
-          configConstraints_ = graph_.lock ()->configConstraint (thisNode);
+          configConstraints_ = graph_.lock ()->configConstraint (wkPtr_.lock ());
         }
         return configConstraints_;
       }

src/manipulation-planner.cc

@@ -102,13 +102,13 @@ namespace hpp {
     {
       graph::GraphPtr_t graph = problem_.constraintGraph ();
       // Select next node in the constraint graph.
-      graph::Nodes_t nodes = graph->getNode (*q_near);
-      graph::Edges_t edges = graph->chooseEdge (nodes);
-      ConstraintSetPtr_t constraint = graph->configConstraint (edges);
+      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_)) {
-        addFailure (PROJECTION, edges);
+        addFailure (PROJECTION, edge);
         SuccessStatistics& ss = constraint->configProjector ()->statistics ();
         if (ss.nbFailure () > ss.nbSuccess ()) {
           hppDout (warning, constraint->name () << " fails often." << std::endl << ss);
@@ -120,31 +120,27 @@ namespace hpp {
       core::SteeringMethodPtr_t sm (problem().steeringMethod());
       core::PathPtr_t path = (*sm) (*q_near, qProj_);
       if (!path) {
-        addFailure (STEERING_METHOD, edges);
+        addFailure (STEERING_METHOD, edge);
         return false;
       }
       core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
       pathValidation->validate (path, false, validPath);
       if (validPath->length () == 0)
-        addFailure (PATH_VALIDATION, edges);
+        addFailure (PATH_VALIDATION, edge);
       else
         extendStatistics_.addSuccess ();
       return true;
     }
 
-    void ManipulationPlanner::addFailure (TypeOfFailure t, const graph::Edges_t& edges)
+    void ManipulationPlanner::addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge)
     {
-      EdgesReasonMap::iterator it = failureReasons_.find (edges);
+      EdgeReasonMap::iterator it = failureReasons_.find (edge);
       if (it == failureReasons_.end ()) {
-        std::string edgesStr = "(";
-        for (graph::Edges_t::const_iterator itEdge = edges.begin();
-            itEdge != edges.end (); itEdge++)
-          edgesStr += (*itEdge)->name () + " / ";
-        edgesStr += ")";
-        Reasons r (SuccessBin::createReason ("Projection for " + edgesStr),
-                   SuccessBin::createReason ("SteeringMethod for " + edgesStr),
-                   SuccessBin::createReason ("PathValidation returned length 0 for " + edgesStr));
-        failureReasons_.insert (EdgesReasonPair (edges, r));
+        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));
+        failureReasons_.insert (EdgeReasonPair (edge, r));
         extendStatistics_.addFailure (r.get (t));
         return;
       }
@@ -160,10 +156,7 @@ namespace hpp {
       core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
       core::PathPtr_t path, validPath;
       graph::GraphPtr_t graph = problem_.constraintGraph ();
-      graph::Nodes_t n1, n2;
-      graph::Edges_t edges;
       bool connectSucceed = false;
-      std::vector< graph::Edges_t > possibleEdges;
       for (core::Nodes_t::const_iterator itn1 = nodes.begin ();
           itn1 != nodes.end (); ++itn1) {
         ConfigurationPtr_t q1 ((*itn1)->configuration ());

tests/test-constraintgraph.cc

@@ -68,20 +68,13 @@ namespace hpp_test {
 
   GraphComponents components;
   GraphPtr_t graph_;
-  NodeSelectorPtr_t ns1;
-  NodeSelectorPtr_t ns2;
-  NodePtr_t n11;
-  NodePtr_t n12;
-  NodePtr_t n21;
-  NodePtr_t n22;
-  EdgePtr_t e111;
-  EdgePtr_t e121;
-  EdgePtr_t e112;
-  EdgePtr_t e122;
-  EdgePtr_t e211;
-  EdgePtr_t e221;
-  EdgePtr_t e212;
-  EdgePtr_t e222;
+  NodeSelectorPtr_t ns;
+  NodePtr_t n1;
+  NodePtr_t n2;
+  EdgePtr_t e11;
+  EdgePtr_t e21;
+  EdgePtr_t e12;
+  EdgePtr_t e22;
 
   void initialize (bool ur5)
   {
@@ -100,25 +93,13 @@ namespace hpp_test {
     graph_ = Graph::create (robot); components.push_back(graph_);
     graph_->maxIterations (20);
     graph_->errorThreshold (1e-4);
-    ns1 = graph_->createNodeSelector(); components.push_back(ns1);
-    if (!ur5)
-      ns2 = graph_->createNodeSelector(); components.push_back(ns2);
-    n11 = ns1->createNode (); components.push_back(n11);
-    n12 = ns1->createNode (); components.push_back(n12);
-    if (!ur5) {
-      n21 = ns2->createNode (); components.push_back(n21);
-      n22 = ns2->createNode (); components.push_back(n22);
-    }
-    e111 = n11->linkTo (n11); components.push_back(e111);
-    e121 = n12->linkTo (n11); components.push_back(e121);
-    e112 = n11->linkTo (n12); components.push_back(e112);
-    e122 = n12->linkTo (n12); components.push_back(e122);
-    if (!ur5) {
-      e211 = n21->linkTo (n21); components.push_back(e211);
-      e221 = n22->linkTo (n21); components.push_back(e221);
-      e212 = n21->linkTo (n22); components.push_back(e212);
-      e222 = n22->linkTo (n22); components.push_back(e222);
-    }
+    ns = graph_->createNodeSelector(); components.push_back(ns);
+    n1 = ns->createNode (); components.push_back(n1);
+    n2 = ns->createNode (); components.push_back(n2);
+    e11 = n1->linkTo (n1); components.push_back(e11);
+    e21 = n2->linkTo (n1); components.push_back(e21);
+    e12 = n1->linkTo (n2); components.push_back(e12);
+    e22 = n2->linkTo (n2); components.push_back(e22);
 
     q1 = Configuration_t::Zero(6);
     q2 = Configuration_t::Zero(6);
@@ -143,34 +124,19 @@ BOOST_AUTO_TEST_CASE (GraphStructure)
   }
 
   // Test function Graph::getEdge
-  Nodes_t from, to;
-  from.push_back (n11);
-  from.push_back (n22);
-  to.push_back (n12);
-  to.push_back (n22);
-  std::vector <Edges_t> checkPossibleEdges,
-                        possibleEdges = graph_->getEdge (from, to);
-  do {
-    Edges_t edges;
-    edges.push_back (e112);
-    edges.push_back (e222);
-    checkPossibleEdges.push_back (edges);
-  } while (0);
-  for (size_t i = 0; i < possibleEdges.size(); i++) {
-    for (size_t j = 0; j < possibleEdges[i].size(); j++)
-      BOOST_CHECK_MESSAGE (possibleEdges[i][j] == checkPossibleEdges[i][j],
-          "i = " << i << " and j = " << j);
-  }
+  NodePtr_t from(n1), to(n2);
+  Edges_t checkPossibleEdges,
+          possibleEdges = graph_->getEdges (from, to);
+  checkPossibleEdges.push_back (e12);
+  for (size_t j = 0; j < possibleEdges.size(); j++)
+    BOOST_CHECK_MESSAGE (possibleEdges[j] == checkPossibleEdges[j],
+        "Possible edge j = " << j);
 
   Configuration_t cfg;
-  Nodes_t nodes = graph_->getNode (cfg);
-  BOOST_CHECK (nodes.size() == 2);
-  BOOST_CHECK (nodes[0] == n11);
-  BOOST_CHECK (nodes[1] == n21);
-  Edges_t edges = graph_->chooseEdge (nodes);
-  BOOST_CHECK (edges.size() == 2);
-  BOOST_CHECK (edges[0]->from() == n11);
-  BOOST_CHECK (edges[1]->from() == n21);
+  NodePtr_t node = graph_->getNode (cfg);
+  BOOST_CHECK (node == n1);
+  EdgePtr_t edge = graph_->chooseEdge (node);
+  BOOST_CHECK (edge->from() == n1);
 }
 
 #ifdef TEST_UR5
@@ -201,7 +167,7 @@ BOOST_AUTO_TEST_CASE (ConstraintSets)
   BOOST_CHECK (res == expectedRes);
 
   //graph_->addNumericalConstraint (com);
-  n11->addNumericalConstraint (pos, Equality::create ());
+  n1->addNumericalConstraint (pos, Equality::create ());
 }
 
 BOOST_AUTO_TEST_CASE (PathValidationTest)
@@ -211,19 +177,17 @@ BOOST_AUTO_TEST_CASE (PathValidationTest)
   ProblemPtr_t pb = new Problem (robot);
   BOOST_CHECK(robot->configSize() == 6);
   pb->constraintGraph (graph_);
-  Nodes_t nodes11; nodes11.push_back (n11);
-  Nodes_t nodes12; nodes12.push_back (n12);
-  ConstraintSetPtr_t constraintn11 = graph_->configConstraint (nodes11);
-  ConstraintSetPtr_t constraintn12 = graph_->configConstraint (nodes12);
-  BOOST_CHECK ( constraintn11->isSatisfied (q1));
-  BOOST_CHECK (!constraintn11->isSatisfied (q2));
-  BOOST_CHECK ( constraintn12->isSatisfied (q2));
+  ConstraintSetPtr_t constraintn1 = graph_->configConstraint (n1);
+  ConstraintSetPtr_t constraintn2 = graph_->configConstraint (n2);
+  BOOST_CHECK ( constraintn1->isSatisfied (q1));
+  BOOST_CHECK (!constraintn1->isSatisfied (q2));
+  BOOST_CHECK ( constraintn2->isSatisfied (q2));
   PathPtr_t p = (*pb->steeringMethod ())(ConfigurationIn_t(q1),ConfigurationIn_t(q2)),
             validp;
-  Nodes_t nq1 = graph_->getNode (q1);
-  Nodes_t nq2 = graph_->getNode (q2);
-  BOOST_CHECK (nq1.size() == 1); BOOST_CHECK (nq1[0] == n11);
-  BOOST_CHECK (nq2.size() == 1); BOOST_CHECK (nq2[0] == n12);
+  NodePtr_t nq1 = graph_->getNode (q1);
+  NodePtr_t nq2 = graph_->getNode (q2);
+  BOOST_CHECK (nq1 == n1);
+  BOOST_CHECK (nq2 == n2);
   GraphPathValidationPtr_t pv = pb->pathValidation ();
   BOOST_CHECK (pv);
   if (!pv->validate(p, false, validp)) {