diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh index be27b5f8675bb91865bd133d6f9b6fa47051d25f..cfd5711d0af9df3fca96d02534c2c650a5a8ff6a 100644 --- a/include/hpp/manipulation/graph/edge.hh +++ b/include/hpp/manipulation/graph/edge.hh @@ -116,6 +116,10 @@ namespace hpp { /// \return The initialized projector. ConstraintSetPtr_t configConstraint() const; + void setShort (bool isShort) { + isShort_ = isShort; + } + protected: /// Initialization of the object. void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const NodeWkPtr_t& from, @@ -136,6 +140,8 @@ namespace hpp { /// Print the object in a stream. virtual std::ostream& print (std::ostream& os) const; + bool isShort_; + private: typedef Cache < ConstraintSetPtr_t > Constraint_t; diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 43a835ea58ead2ff6927b8f7a06227fd3ee6f958..84c6e4b874bd16bcb10e292b6d4ad53ef8ecf88a 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -34,7 +34,8 @@ namespace hpp { namespace graph { Edge::Edge (const std::string& name, const core::SteeringMethodPtr_t& steeringMethod) : - GraphComponent (name), pathConstraints_ (new Constraint_t()), + GraphComponent (name), isShort_ (false), + pathConstraints_ (new Constraint_t()), configConstraints_ (new Constraint_t()), steeringMethod_ (steeringMethod->copy ()) {} @@ -275,10 +276,8 @@ namespace hpp { ConstraintSetPtr_t c = configConstraint (); ConfigProjectorPtr_t proj = c->configProjector (); proj->rightHandSideFromConfig (qoffset); - if (c->apply (q)) { - return true; - } - assert (proj); + if (isShort_) q = qoffset; + if (c->apply (q)) return true; ::hpp::statistics::SuccessStatistics& ss = proj->statistics (); if (ss.nbFailure () > ss.nbSuccess ()) { hppDout (warning, c->name () << " fails often." << std::endl << ss); @@ -478,8 +477,8 @@ namespace hpp { cp->updateRightHandSide (); // Eventually, do the projection. - if (cs->apply (q)) - return true; + if (isShort_) q = q_offset; + if (cs->apply (q)) return true; ::hpp::statistics::SuccessStatistics& ss = cp->statistics (); if (ss.nbFailure () > ss.nbSuccess ()) { hppDout (warning, cs->name () << " fails often." << std::endl << ss);