Add map of grasp (=pair<gripper,handle>) to disable the collisions between...

Add map of grasp (=pair<gripper,handle>) to disable the collisions between gripper and handle when a grasping constraint is set.

include/hpp/manipulation/problem-solver.hh

@@ -31,12 +31,15 @@ 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 ()
       {
       }
       ProblemSolver () : core::ProblemSolver (), robot_ (),
-	robotsAndObjects_ ()
+	robotsAndObjects_ (), graspsMap_()
 	{
 	}
       /// Set robot
@@ -84,6 +87,27 @@ namespace hpp {
       ObjectPtr_t object (const std::string& name) const;
       /// \}
 
+      /// Add grasp
+      void addGrasp( std::string graspName, model::GripperPtr_t gripper,
+                       HandlePtr_t handle) 
+      {
+        Grasp_t* ptr = new Grasp_t (gripper, handle);
+	GraspPtr_t shPtr (ptr);
+        graspsMap_[graspName] = shPtr;
+      }
+   
+      /// get grapsMap
+      GraspsMap_t& grasps()
+      {
+        return graspsMap_;
+      }
+ 
+      /// get graps by name
+      ///
+      /// return NULL if no grasp named graspName
+      GraspPtr_t grasp(const std::string& graspName) const;
+
+
       /// Build a composite robot from several robots and objects
       /// \param robotName Name of the composite robot,
       /// \param robotNames Names of the robots stored internally that have
@@ -97,6 +121,7 @@ namespace hpp {
       RobotPtr_t robot_;
       /// Map of single robots to store before building a composite robot.
       RobotsandObjects_t robotsAndObjects_;
+      GraspsMap_t graspsMap_;
     }; // class ProblemSolver
   } // namespace manipulation
 } // namespace hpp

src/problem-solver.cc

@@ -67,5 +67,15 @@ namespace hpp {
       return object;
     }
 
+    ProblemSolver::GraspPtr_t ProblemSolver::grasp (const std::string& graspName) const
+    {
+      GraspsMap_t::const_iterator it =
+	graspsMap_.find (graspName);
+      if (it == graspsMap_.end ()) {
+        GraspPtr_t grasp;
+	return grasp;
+      }
+      return it->second;
+    }
   } // namespace manipulation
 } // namespace hpp

src/robot.cc

@@ -196,60 +196,18 @@ namespace hpp {
 	    hppDout (info, "Joint " + joint2->name () <<
 		     " has no hpp::model::Body.");
 	    } else {
-  	      if (!isCollisionPairDisabled (joint1, joint2)) {
-	        hppDout (info, "add collision pairs: ("  << joint1->name() <<
-                                                "," << joint2->name() << ")");
-	        hpp::model::Device::addCollisionPairs (joint1, joint2,
-                                                        hpp::model::COLLISION);
-	        hpp::model::Device::addCollisionPairs (joint1, joint2, 
-                                                        hpp::model::DISTANCE);
-              }
+	      hppDout (info, "add collision pairs: ("  << joint1->name() <<
+                                              "," << joint2->name() << ")");
+	      hpp::model::Device::addCollisionPairs (joint1, joint2,
+                                                      hpp::model::COLLISION);
+	      hpp::model::Device::addCollisionPairs (joint1, joint2, 
+                                                      hpp::model::DISTANCE);
             }
 	  }
         }
       }
     }
 
-    bool Robot::isCollisionPairDisabled(JointPtr_t& joint1, JointPtr_t& joint2)
-    {
-      // Disable collision if a joint is an handle and the other is a 
-      // disabledCollision from a gripper
-      for (Handles_t::iterator itHandle = handles_.begin();
-             itHandle != handles_.end() ; itHandle++) {
-        if ( itHandle->second->joint() == joint1 ) {
-          for (Grippers_t::iterator itGripper = grippers_.begin();
-                 itGripper != grippers_.end() ; itGripper++) {
-            model::JointVector_t joints = itGripper->second
-                                           ->getDisabledCollisions();
-            for (model::JointVector_t::iterator itJoint = joints.begin() ;
-                   itJoint != joints.end() ; itJoint++ ) {
-              if ( joint2 == *(itJoint) ) {
-                hppDout (info, "Disabled collision between "<< 
-                           joint1->name() << " and " << joint2->name());
-                return true;
-              }
-            }
-          }
-        }
-        if ( itHandle->second->joint() == joint2 ) {
-          for (Grippers_t::iterator itGripper = grippers_.begin();
-                 itGripper != grippers_.end() ; itGripper++) {
-              model::JointVector_t joints = itGripper->second
-                                           ->getDisabledCollisions();
-            for (model::JointVector_t::iterator itJoint = joints.begin() ;
-                   itJoint != joints.end() ; itJoint++ ) {
-              if ( joint1 == *(itJoint) ) {
-                hppDout (info, "Disabled collision between "<< 
-                           joint1->name() << " and " << joint2->name());
-                return true;
-              }
-            }
-          }
-        }
-      }
-      return false;
-    }
-
     std::ostream& Robot::print (std::ostream& os) const
     {
       parent_t::print (os);