diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
index df3e4beaa53a60307df190f0184e5bb27f421a96..1073a27790b79082780c5a0150e9924a4bae31b4 100644
--- a/include/hpp/manipulation/path-planner/states-path-finder.hh
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -176,7 +176,16 @@ namespace hpp {
           graph::Edges_t getTransitionList2 (const GraphSearchData& data) const;
 
           /// Step 3 of the algorithm
+
+          // check that the solver either contains exactly same constraint
+          // or a constraint with similar parametrizable form
+          // constraint/both and constraint/complement
           bool contains (const Solver_t& solver, const ImplicitPtr_t& c) const;
+
+          // check that the solver either contains exactly same constraint
+          // or a stricter version of the constraint
+          // constraint/both stricter than constraint and stricter than constraint/complement
+          bool containsStricter (const Solver_t& solver, const ImplicitPtr_t& c) const;
           bool checkConstantRightHandSide (size_type index);
           bool buildOptimizationProblem (const graph::Edges_t& transitions);
           bool buildOptimizationProblem2 (const graph::Edges_t& transitions);
@@ -214,9 +223,14 @@ namespace hpp {
           std::map < std::string, std::size_t > index_;
 
           /// associative map that stores pairs of constraints of the form
-          /// (constraint, constraint/hold)
+          /// (constraint/complement, constraint/hold)
           std::map <ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
 
+          /// associative map that stores pairs of constraints of either form
+          /// (constraint, constraint/hold)
+          /// or (constraint/complement, constraint/hold)
+          std::map <ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
+
           mutable OptimizationData* optData_;
           /// Index of the sequence of states
           std::size_t idxSol_ = 0;
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 39fb070d691d06d98c6eac004aafd6c3cb9b907a..9a90d241bedc2a2ef7c8d5c3302c37e8147cf8f0 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -104,7 +104,7 @@ namespace hpp {
         goalDefinedByConstraints_(false),
         q1_(0x0), q2_(0x0), configList_(), idxConfigList_(0),
         nTryConfigList_(0), solved_(false), interrupt_(false),
-        weak_()
+        weak_(), stricterConstraints_()
       {
         gatherGraphConstraints ();
         inStateProblem_ = core::Problem::create(problem_->robot());
@@ -298,6 +298,12 @@ namespace hpp {
             }
           }
         }
+        // both is the intersection of both constraint and constraint/complement
+        for (ConstraintsAndComplements_t::const_iterator it(cac.begin ());
+              it != cac.end (); ++it) {
+          stricterConstraints_ [it->constraint] = it->both;
+          stricterConstraints_ [it->complement] = it->both;
+        }
       }
 
       bool StatesPathFinder::findTransitions (GraphSearchData& d) const
@@ -676,6 +682,17 @@ namespace hpp {
         return false;
       }
 
+      bool StatesPathFinder::containsStricter
+      (const Solver_t& solver, const ImplicitPtr_t& c) const
+      {
+        if (solver.contains (c)) return true;
+        std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
+          (stricterConstraints_.find (c));
+        if (it != stricterConstraints_.end() && solver.contains (it->second))
+          return true;
+        return false;
+      }
+
       bool StatesPathFinder::buildOptimizationProblem
         (const graph::Edges_t& transitions)
       {
@@ -803,7 +820,7 @@ namespace hpp {
         }
         // Add in the constraints for the goal
         for (auto goalConstraint: goalConstraints_) {
-          if (!contains(d.solvers [d.N-1], goalConstraint)) {
+          if (!containsStricter(d.solvers [d.N-1], goalConstraint)) {
             d.solvers [d.N-1].add(goalConstraint);
             hppDout(info, "Adding goal constraint " << goalConstraint->function().name()
                                   << " to solver for waypoint" << d.N);