diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh index 9e712646ae2b8a3e8ed4c717fbc29466f049584a..6169b165dac3e4884671c22cf8a6260b2ff79300 100644 --- a/include/hpp/manipulation/graph/graph.hh +++ b/include/hpp/manipulation/graph/graph.hh @@ -98,6 +98,20 @@ namespace hpp { bool getConfigErrorForNode (ConfigurationIn_t config, const NodePtr_t& node, vector_t& error); + /// Get error of a config with respect to an edge constraint + /// + /// \param config Configuration, + /// \param edge edge containing the constraint to check config against + /// \retval error the error of the edge constraint for the + /// configuration + /// \return whether the configuration can be a start point of a path + // of the edge + /// Call core::ConfigProjector::rightHandSideFromConfig with + /// input configuration and method core::ConstraintSet::isSatisfied + /// for the edge constraint. + bool getConfigErrorForEdge (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. diff --git a/src/graph/graph.cc b/src/graph/graph.cc index fc8385ec35615090cfc8401ac919bf15f6919e5b..d76e518b32f78d7cad6cf83fd8b247dd5dbb20d2 100644 --- a/src/graph/graph.cc +++ b/src/graph/graph.cc @@ -122,6 +122,15 @@ namespace hpp { return configConstraint (node)->isSatisfied (config, error); } + bool Graph::getConfigErrorForEdge (ConfigurationIn_t config, + const EdgePtr_t& edge, vector_t& error) + { + ConstraintSetPtr_t cs (pathConstraint (edge)); + ConfigProjectorPtr_t cp (cs->configProjector ()); + if (cp) cp->rightHandSideFromConfig (config); + return cs->isSatisfied (config, error); + } + ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge) { return edge->configConstraint ();