diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh index a7314ea42dad02b780e189f2476a0eb8ba3a85da..9d1b67c7916c827b621c566eb6ee1e6d642f5077 100644 --- a/include/hpp/manipulation/graph/graph.hh +++ b/include/hpp/manipulation/graph/graph.hh @@ -201,6 +201,20 @@ namespace hpp { (ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t& edge, vector_t& error) const; + /// Get error of a config with respect to the target of 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 getConfigErrorForEdgeTarget + (ConfigurationIn_t leafConfig, ConfigurationIn_t config, + const EdgePtr_t& edge, vector_t& error) const; + /// 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 7d397a98e0dbb0495093f0ce5c9d2b61be1932a1..f50d5af9ecc974bcf573adcd467c6b7759a3b7c2 100644 --- a/src/graph/graph.cc +++ b/src/graph/graph.cc @@ -204,6 +204,16 @@ namespace hpp { return cs->isSatisfied (config, error); } + bool Graph::getConfigErrorForEdgeTarget + (ConfigurationIn_t leafConfig, ConfigurationIn_t config, + const EdgePtr_t& edge, vector_t& error) const + { + ConstraintSetPtr_t cs (configConstraint (edge)); + ConfigProjectorPtr_t cp (cs->configProjector ()); + if (cp) cp->rightHandSideFromConfig (leafConfig); + return cs->isSatisfied (config, error); + } + bool Graph::getConfigErrorForEdgeLeaf (ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t& edge, vector_t& error) const