diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh index 6169b165dac3e4884671c22cf8a6260b2ff79300..dd8b0062cf7e2dfbea0f12fafb311db7e71c89c4 100644 --- a/include/hpp/manipulation/graph/graph.hh +++ b/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. diff --git a/src/graph/graph.cc b/src/graph/graph.cc index d76e518b32f78d7cad6cf83fd8b247dd5dbb20d2..a2c3ed1cdc42eab83f5c6b4e74308311870f8ec8 100644 --- a/src/graph/graph.cc +++ b/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 ();