Add a method in ProblemSolver to create a placement constraint

  - also create complement constraint.

include/hpp/manipulation/problem-solver.hh

@@ -94,12 +94,13 @@ namespace hpp {
         GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
 
 	/// Create placement constraint
-	/// \param name name of the first 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& jointName,
-					const std::string& triangleName,
-					const std::string& envContactName);
+					const std::string& surface1,
+					const std::string& surface2);
 
         /// Reset constraint set and put back the disable collisions
         /// between gripper and handle

src/problem-solver.cc

@@ -22,6 +22,8 @@
 
 #include <hpp/model/gripper.hh>
 
+#include <hpp/constraints/static-stability.hh>
+
 #include <hpp/core/roadmap.hh>
 
 #include "hpp/manipulation/device.hh"
@@ -82,6 +84,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_)