Allow collision checking before computing grasp quality to be toggled

CMakeLists.txt

@@ -98,6 +98,8 @@ find_package(graspit)
    ApproachToContact.srv
    FindInitialContact.srv
    DynamicAutoGraspComplete.srv
+   GetCheckCollision.srv
+   SetCheckCollision.srv
  )
 
 

include/graspit_interface.h

@@ -46,6 +46,8 @@
 #include <graspit_interface/ApproachToContact.h>
 #include <graspit_interface/FindInitialContact.h>
 #include <graspit_interface/DynamicAutoGraspComplete.h>
+#include <graspit_interface/GetCheckCollision.h>
+#include <graspit_interface/SetCheckCollision.h>
 
 // ActionServer includes
 #include <graspit_interface/PlanGraspsAction.h>
@@ -77,6 +79,9 @@ class GraspitInterface : public QObject, public Plugin
   ros::ServiceServer getDynamics_srv;
   ros::ServiceServer setDynamics_srv;
 
+  ros::ServiceServer getCheckCollision_srv;
+  ros::ServiceServer setCheckCollision_srv;
+
   ros::ServiceServer autoGrasp_srv;
   ros::ServiceServer autoOpen_srv;
   ros::ServiceServer forceRobotDOF_srv;
@@ -112,6 +117,7 @@ class GraspitInterface : public QObject, public Plugin
   SimAnnPlanner *mPlanner;
 
   bool firstTimeInMainLoop;
+  bool checkCollision;
 
   // Service callbacks
   bool getRobotCB(graspit_interface::GetRobot::Request &request,
@@ -201,6 +207,12 @@ class GraspitInterface : public QObject, public Plugin
   bool dynamicAutoGraspCompleteCB(graspit_interface::DynamicAutoGraspComplete::Request &request,
                                   graspit_interface::DynamicAutoGraspComplete::Response &response);
 
+  bool getCheckCollisionCB(graspit_interface::GetCheckCollision::Request &request,
+                           graspit_interface::GetCheckCollision::Response &response);
+
+  bool setCheckCollisionCB(graspit_interface::SetCheckCollision::Request &request,
+                           graspit_interface::SetCheckCollision::Response &response);
+
   //ActionServer callbacks
   void PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &goal);
 

src/graspit_interface.cpp

@@ -58,6 +58,9 @@ int GraspitInterface::init(int argc, char** argv)
     getDynamics_srv = nh->advertiseService("getDynamics", &GraspitInterface::getDynamicsCB, this);
     setDynamics_srv = nh->advertiseService("setDynamics", &GraspitInterface::setDynamicsCB, this);
 
+    getCheckCollision_srv = nh->advertiseService("getCheckCollision", &GraspitInterface::getCheckCollisionCB, this);
+    setCheckCollision_srv = nh->advertiseService("setCheckCollision", &GraspitInterface::setCheckCollisionCB, this);
+
     autoGrasp_srv = nh->advertiseService("autoGrasp", &GraspitInterface::autoGraspCB, this);
     autoOpen_srv = nh->advertiseService("autoOpen", &GraspitInterface::autoOpenCB, this);
 
@@ -88,6 +91,7 @@ int GraspitInterface::init(int argc, char** argv)
     plan_grasps_as->start();
 
     firstTimeInMainLoop = true;
+    checkCollision = true;
 
     mPlanner = NULL;
     mHandObjectState = NULL;
@@ -388,6 +392,20 @@ bool GraspitInterface::setDynamicsCB(graspit_interface::SetDynamics::Request &re
     return true;
 }
 
+bool GraspitInterface::getCheckCollisionCB(graspit_interface::GetCheckCollision::Request &request,
+                graspit_interface::GetCheckCollision::Response &response)
+{
+    response.collisionCheckEnabled = checkCollision;
+    return true;
+}
+
+bool GraspitInterface::setCheckCollisionCB(graspit_interface::SetCheckCollision::Request &request,
+                graspit_interface::SetCheckCollision::Response &response)
+{
+    checkCollision = request.enableCollisionCheck;
+    return true;
+}
+
 bool GraspitInterface::autoGraspCB(graspit_interface::AutoGrasp::Request &request,
                        graspit_interface::AutoGrasp::Response &response)
 {
@@ -643,15 +661,20 @@ bool GraspitInterface::computeQualityCB(graspit_interface::ComputeQuality::Reque
                                          graspit_interface::ComputeQuality::Response &response)
 {
     CollisionReport colReport;
-    // first test whether the hand is in collision now
-    int numCols = graspitCore->getWorld()->getCollisionReport(&colReport);
-    // if it is in collision, then there should be no reason to calculate the quality
-    if(numCols>0){
-        response.result = response.RESULT_COLLISION;
-        response.epsilon = -1.0;
-        response.volume = -1.0;
-        return true;
+
+    if (checkCollision)
+    {
+        // first test whether the hand is in collision now
+        int numCols = graspitCore->getWorld()->getCollisionReport(&colReport);
+        // if it is in collision, then there should be no reason to calculate the quality
+        if(numCols>0){
+            response.result = response.RESULT_COLLISION;
+            response.epsilon = -1.0;
+            response.volume = -1.0;
+            return true;
+        }
     }
+
     Hand *mHand =graspitCore->getWorld()->getHand(request.id);
     if (mHand==NULL)
     {

srv/GetCheckCollision.srv

@@ -0,0 +1,3 @@
+
+---
+bool collisionCheckEnabled

srv/SetCheckCollision.srv

@@ -0,0 +1,2 @@
+bool enableCollisionCheck
+---