Change graspsMap key from string to DifferentialFunction.

  add virtual function resetConstraints to deal with collisions.
  add virtual function addConstraintToConfigProjector to deal with gripper
  disable collisions in the ProblemSolver.

include/hpp/manipulation/fwd.hh

@@ -66,6 +66,11 @@ namespace hpp {
     typedef std::vector <ObjectPtr_t> Objects_t;
     typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t;
     typedef core::ConstraintPtr_t ConstraintPtr_t;
+    typedef core::ConstraintSet ConstraintSet;
+    
+    typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
+    typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
+    typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
   } // namespace manipulation
 } // namespace hpp
 

include/hpp/manipulation/problem-solver.hh

@@ -31,9 +31,6 @@ namespace hpp {
     public:
       typedef core::ProblemSolver parent_t;
       typedef std::vector <std::string> Names_t;
-      typedef std::pair< model::GripperPtr_t, HandlePtr_t> Grasp_t;
-      typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
-      typedef std::map <const std::string, GraspPtr_t> GraspsMap_t;
       /// Destructor
       virtual ~ProblemSolver ()
       {
@@ -88,12 +85,13 @@ namespace hpp {
       /// \}
 
       /// Add grasp
-      void addGrasp( std::string graspName, model::GripperPtr_t gripper,
-                       HandlePtr_t handle) 
+      void addGrasp( const DifferentiableFunctionPtr_t& constraint,
+                     const model::GripperPtr_t& gripper,
+                     const HandlePtr_t& handle) 
       {
         Grasp_t* ptr = new Grasp_t (gripper, handle);
 	GraspPtr_t shPtr (ptr);
-        graspsMap_[graspName] = shPtr;
+        graspsMap_[constraint] = shPtr;
       }
    
       /// get grapsMap
@@ -105,8 +103,17 @@ namespace hpp {
       /// get graps by name
       ///
       /// return NULL if no grasp named graspName
-      GraspPtr_t grasp(const std::string& graspName) const;
+      GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
 
+      /// Reset constraint set and put back the disable collisions
+      /// between gripper and handle
+      virtual void resetConstraints ();
+
+      /// Add differentialFunction to the config projector
+      /// Build the config projector if not constructed
+      virtual void addConstraintToConfigProjector(
+                          const std::string& constraintName,
+                          const DifferentiableFunctionPtr_t& constraint);
 
       /// Build a composite robot from several robots and objects
       /// \param robotName Name of the composite robot,

include/hpp/manipulation/robot.hh

@@ -160,9 +160,6 @@ namespace hpp {
       /// Add Collision pairs between the robots
       void addCollisions(const DeviceConstPtr_t& device);
 
-      /// Check if joint1 joint2 is a couple gripper-handle 
-      bool isCollisionPairDisabled(JointPtr_t& joint1, JointPtr_t& joint2);
-
       Devices_t robots_;
       /// Set of objects
       Objects_t objects_;

src/problem-solver.cc

@@ -20,6 +20,8 @@
 #include <hpp/manipulation/object.hh>
 #include <hpp/manipulation/problem-solver.hh>
 #include <hpp/manipulation/robot.hh>
+#include <hpp/model/gripper.hh>
+
 
 namespace hpp {
   namespace manipulation {
@@ -67,15 +69,58 @@ namespace hpp {
       return object;
     }
 
-    ProblemSolver::GraspPtr_t ProblemSolver::grasp (const std::string& graspName) const
+    GraspPtr_t ProblemSolver::grasp (
+                      const DifferentiableFunctionPtr_t& constraint) const
     {
       GraspsMap_t::const_iterator it =
-	graspsMap_.find (graspName);
+	graspsMap_.find (constraint);
       if (it == graspsMap_.end ()) {
         GraspPtr_t grasp;
 	return grasp;
       }
       return it->second;
     }
+
+    void ProblemSolver::resetConstraints ()
+    {
+      if (robot_)
+	constraints_ = ConstraintSet::create (robot_, 
+                                              "Default constraint set");    
+      GraspsMap_t graspsMap = grasps();
+      for (GraspsMap_t::const_iterator itGrasp = graspsMap.begin() ;  
+             itGrasp != graspsMap.end() ; itGrasp++) {
+        GraspPtr_t grasp = itGrasp->second;
+        GripperPtr_t gripper = grasp->first;
+        HandlePtr_t handle = grasp->second;
+        JointPtr_t joint = handle->joint();
+        model::JointVector_t joints = gripper->getDisabledCollisions();
+        for (model::JointVector_t::iterator itJoint = joints.begin() ;
+               itJoint != joints.end() ; itJoint++ ) {
+          robot()->addCollisionPairs(joint, *itJoint, hpp::model::COLLISION);
+          robot()->addCollisionPairs(joint, *itJoint, hpp::model::DISTANCE);
+        }
+      }
+    }
+   
+    void ProblemSolver::addConstraintToConfigProjector (
+                          const std::string& constraintName,
+                          const DifferentiableFunctionPtr_t& constraint)
+    {
+      core::ProblemSolver::addConstraintToConfigProjector(constraintName,
+                                                          constraint);
+      if ( grasp(constraint) ) {
+        GripperPtr_t gripper = grasp(constraint)->first;
+        HandlePtr_t handle = grasp(constraint)->second;
+        JointPtr_t joint1 = handle->joint();
+        model::JointVector_t joints = gripper->getDisabledCollisions();
+        for (model::JointVector_t::iterator itJoint = joints.begin() ;
+               itJoint != joints.end() ; itJoint++ ) {
+          robot()->removeCollisionPairs(joint1, *itJoint,
+                                        hpp::model::COLLISION);
+          robot()->removeCollisionPairs(joint1, *itJoint,
+                                        hpp::model::DISTANCE);
+        }
+      }
+    }
   } // namespace manipulation
 } // namespace hpp