Add a method that computes the error of a config wrt a leaf.

  - Graph::getConfigErrorForEdgeLeaf.

include/hpp/manipulation/graph/graph.hh

@@ -112,6 +112,20 @@ namespace hpp {
 	  bool getConfigErrorForEdge (ConfigurationIn_t config,
 				      const EdgePtr_t& edge, vector_t& error);
 
+	  /// Get error of a config with respect to an edge foliation leaf
+	  ///
+	  /// \param leafConfig Configuration that determines the foliation leaf
+	  /// \param config Configuration the error of which is computed
+	  /// \retval error the error
+	  /// \return whether config can be the end point of a path of the edge
+	  ///         starting at leafConfig
+	  /// Call methods core::ConfigProjector::rightHandSideFromConfig with
+	  /// leafConfig and then core::ConstraintSet::isSatisfied with config.
+	  /// on the edge constraints.
+	  bool getConfigErrorForEdgeLeaf
+	    (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+	     const EdgePtr_t& edge, vector_t& error);
+
           /// Constraint to project a path.
           /// \param edge a list of edges defining the foliation.
           /// \return The constraint.

src/graph/graph.cc

@@ -131,6 +131,16 @@ namespace hpp {
 	return cs->isSatisfied (config, error);
       }
 
+      bool Graph::getConfigErrorForEdgeLeaf
+      (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+       const EdgePtr_t& edge, vector_t& error)
+      {
+	ConstraintSetPtr_t cs (pathConstraint (edge));
+	ConfigProjectorPtr_t cp (cs->configProjector ());
+	if (cp) cp->rightHandSideFromConfig (leafConfig);
+	return cs->isSatisfied (config, error);
+      }
+
       ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
       {
         return edge->configConstraint ();