Add method Graph::getConfigErrorForNode

  This method returns the error of a configuration for the constraint stored
  in a node as a vector.

include/hpp/manipulation/graph/graph.hh

@@ -67,6 +67,18 @@ namespace hpp {
           /// \return The constraint.
           ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge);
 
+	  /// Get error of a config with respect to a node constraint
+	  ///
+	  /// \param config Configuration,
+	  /// \param node node containing the constraint to check config against
+	  /// \retval error the error of the node constraint for the
+	  ///         configuration
+	  /// \return whether the configuration belongs to the node.
+	  /// Call method core::ConstraintSet::isSatisfied for the node
+	  /// constraints.
+	  bool getConfigErrorForNode (ConfigurationIn_t config,
+				      const NodePtr_t& node, vector_t& error);
+
           /// Constraint to project a path.
           /// \param edge a list of edges defining the foliation.
           /// \return The constraint.

src/graph/graph.cc

@@ -98,6 +98,12 @@ namespace hpp {
         return node->configConstraint ();
       }
 
+      bool Graph::getConfigErrorForNode (ConfigurationIn_t config,
+					 const NodePtr_t& node, vector_t& error)
+      {
+	return configConstraint (node)->isSatisfied (config, error);
+      }
+
       ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
       {
         return edge->configConstraint ();