diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 1f220e31d23e9f98814dd24fda57fa6758405ae5..12bc4dc6246cf6e1186c5a333bd46a028e0c308d 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -24,7 +24,8 @@
 
 namespace hpp {
   namespace manipulation {
-    /// Handle symmetric around its x-axis.
+    /// Handle symmetric around its z-axis. The constraint defined for a grasp
+    /// by a gripper is free to rotate around z-axis.
     class AxialHandle : public Handle
     {
     public:
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 089dd0cb736d7611906c76fb4a61401e03aff06a..5d5b89f9c373dfd0a92b6e858d56c69d433cf362 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -90,6 +90,15 @@ namespace hpp {
         /// return NULL if no grasp named graspName
         GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
 
+	/// Create placement constraint
+	/// \param name name of the placement constraint,
+	/// \param triangleName name of the first list of triangles,
+	/// \param envContactName name of the second list of triangles.
+	/// 
+	void createPlacementConstraint (const std::string& name,
+					const std::string& surface1,
+					const std::string& surface2);
+
         /// Reset constraint set and put back the disable collisions
         /// between gripper and handle
         virtual void resetConstraints ();
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 90aea426bf0b5c6b50f053049999eb52bc74e5ac..43a835ea58ead2ff6927b8f7a06227fd3ee6f958 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -340,8 +340,10 @@ namespace hpp {
       {
         assert (waypoint_.first);
         config_ = q;
-        if (!waypoint_.first->applyConstraints (qoffset, config_))
+        if (!waypoint_.first->applyConstraints (qoffset, config_)) {
+	  q = config_;
           return false;
+	}
         bool success = Edge::applyConstraints (config_, q);
         result_ = q;
         return success;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index af40f02de8baa1d8fd6c8a21790165389593a6b7..74c9d182bb1d6ae814aaf4526aae593cae69fe9d 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -22,10 +22,13 @@
 
 #include <hpp/model/gripper.hh>
 
+#include <hpp/constraints/static-stability.hh>
+
 #include <hpp/core/random-shortcut.hh>
 #include <hpp/core/discretized-collision-checking.hh>
 #include <hpp/core/continuous-collision-checking/progressive.hh>
 #include <hpp/core/path-optimization/partial-shortcut.hh>
+#include <hpp/core/roadmap.hh>
 
 #include "hpp/manipulation/device.hh"
 #include "hpp/manipulation/handle.hh"
@@ -132,6 +135,45 @@ namespace hpp {
       return it->second;
     }
 
+    void ProblemSolver::createPlacementConstraint
+    (const std::string& name, const std::string& surface1,
+     const std::string& surface2)
+    {
+      if (!robot_) throw std::runtime_error ("No robot loaded");
+      using constraints::StaticStabilityGravityPtr_t;
+      using constraints::StaticStabilityGravityComplement;
+      using constraints::StaticStabilityGravityComplementPtr_t;
+      std::string complementName (name + "/complement");
+      std::pair < StaticStabilityGravityPtr_t,
+		  StaticStabilityGravityComplementPtr_t > constraints
+	(StaticStabilityGravityComplement::createPair
+	 (name, complementName, robot_));
+      JointAndTriangles_t l = robot_->get <JointAndTriangles_t>	(surface1);
+      if (l.empty ()) throw std::runtime_error
+			("First list of triangles not found.");
+      for (JointAndTriangles_t::const_iterator it = l.begin ();
+	   it != l.end(); ++it) {
+	constraints.first->addObjectTriangle (it->second, it->first);
+      }
+      // Search first robot triangles
+      l = robot_->get <JointAndTriangles_t> (surface2);
+      if (l.empty ()) {
+	// and then environment triangles.
+	l = get <JointAndTriangles_t> (surface2);
+	if (l.empty ()) throw std::runtime_error
+			  ("Second list of triangles not found.");
+      }
+      for (JointAndTriangles_t::const_iterator it = l.begin ();
+	   it != l.end(); ++it) {
+	constraints.first->addFloorTriangle (it->second, it->first);
+      }
+
+      addNumericalConstraint (name, constraints.first);
+      addNumericalConstraint (complementName, constraints.second);
+      comparisonType (complementName, core::Equality::create ());
+    }
+
+
     void ProblemSolver::resetConstraints ()
     {
       if (robot_)
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index af8e9bd319dd994de5ffee3f82742a8a104102d3..9a85d30ea249a7852eb43b4aef8c16e1a8db3d72 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -47,8 +47,8 @@ MACRO(ADD_TESTCASE NAME GENERATED)
 
 ENDMACRO(ADD_TESTCASE)
 
-ADD_TESTCASE (test-constraintgraph FALSE)
 IF (TEST_UR5)
+  ADD_TESTCASE (test-constraintgraph FALSE)
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DTEST_UR5")
   PKG_CONFIG_USE_DEPENDENCY(test-constraintgraph hpp-model-urdf)
 ENDIF ()