diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh index 22920f897c162d1d5d667a6246a0a47a9bb5a0c7..4b1c8bf56438a25bdd137a3050c310e1b1b8876e 100644 --- a/include/hpp/manipulation/graph/graph.hh +++ b/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. diff --git a/src/graph/graph.cc b/src/graph/graph.cc index 66c3c1a9c09943ad83e061f7ad8abee09a55a997..ef48c210095554f26608d315d583e0fed1ccb70d 100644 --- a/src/graph/graph.cc +++ b/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 ();