diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh index c00f49ad65ed11f17dc5fa43841c652e6eaf98b9..90d8cb135b3cd71c85f625e0723fb9434f753906 100644 --- a/include/hpp/manipulation/graph/edge.hh +++ b/include/hpp/manipulation/graph/edge.hh @@ -498,8 +498,8 @@ namespace hpp { /// Generate a reachable configuration in the target state /// - /// \param qStart node containing the configuration defining the right - /// hand side of the edge path constraint, + /// \param qStart configuration defining the right hand side of the + /// edge path constraint, /// \param[in,out] q input configuration used to initialize the /// numerical solver and output configuration lying /// in the target state and reachable along the edge @@ -507,6 +507,17 @@ namespace hpp { virtual bool generateTargetConfig (ConfigurationIn_t qStart, ConfigurationOut_t q) const; + /// Generate a reachable configuration in leaf of target state + /// \param qStart configuration defining the right hand side of the + /// edge path constraint, + /// \param qLeaf configuration used to set the right hand side of + /// the target state foliation. See method + /// \link LevelSetEdge::insertParamConstraint + /// insertParamConstraint\endlink. + bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart, + ConfigurationIn_t qLeaf, + ConfigurationOut_t q) const; + /// \deprecated Use buildTargetConstraint instead virtual ConstraintSetPtr_t buildConfigConstraint() HPP_MANIPULATION_DEPRECATED; @@ -578,9 +589,6 @@ namespace hpp { virtual void initialize (); private: - bool applyConstraintsWithOffset (ConfigurationIn_t qoffset, - ConfigurationIn_t qlevelset, ConfigurationOut_t q) const; - // Parametrizer // NumericalConstraints_t NumericalConstraints_t paramNumericalConstraints_; diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 45b90fcaa5fba5fe4e3f7ad5752401c76483a6b3..412e82fa7df1c90de75dba8222c73b1e29966e29 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -625,9 +625,9 @@ namespace hpp { hppDout (warning, "Edge " << name() << ": Distrib is empty"); return false; } - const Configuration_t& qlevelset = *(distrib ()->configuration ()); + const Configuration_t& qLeaf = *(distrib ()->configuration ()); - return applyConstraintsWithOffset (qStart, qlevelset, q); + return generateTargetConfigOnLeaf (qStart, qLeaf, q); } bool LevelSetEdge::applyConstraints(core::NodePtr_t nStart, @@ -645,14 +645,15 @@ namespace hpp { hppDout (warning, "Edge " << name() << ": Distrib is empty"); return false; } - const Configuration_t& qlevelset = *(distrib ()->configuration ()), + const Configuration_t& qLeaf = *(distrib ()->configuration ()), qStart = *(nStart->configuration ()); - return applyConstraintsWithOffset (qStart, qlevelset, q); + return generateTargetConfigOnLeaf (qStart, qLeaf, q); } - bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qStart, - ConfigurationIn_t qlevelset, ConfigurationOut_t q) const + bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart, + ConfigurationIn_t qLeaf, + ConfigurationOut_t q) const { // First, set the offset. ConstraintSetPtr_t cs = targetConstraint(); @@ -666,7 +667,7 @@ namespace hpp { for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin (); it != paramNumericalConstraints_.end (); ++it) { - cp->rightHandSideFromConfig (*it, qlevelset); + cp->rightHandSideFromConfig (*it, qLeaf); } // Eventually, do the projection.