allowing to disable end effector collision

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -178,9 +178,10 @@ module hpp
     /// \param heuristicName heuristic used to bias sample selection
     /// \param resolution resolution of the octree used to store the samples (a typical value is 0.01 meters)
     /// \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
+    /// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
     void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal,
                  in double x, in double y, in unsigned short samples, in string heuristicName,
-                 in double resolution, in string contactType) raises (Error);
+                 in double resolution, in string contactType,  in double disableEffectorCollision) raises (Error);
 
     /// Specifies a subchain of the robot as a limb, which can be used to create contacts.
     /// A limb must consist in a simple kinematic chain, where every node has only one child
@@ -188,7 +189,9 @@ module hpp
     /// \param id user given name of the new limb
     /// \param heuristicName heuristic used to bias sample selection
     /// \param loadValues whether other values computed for the limb database should be loaded
-    void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues) raises (Error);
+    /// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
+    void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues,
+                         in double disableEffectorCollision) raises (Error);
 
     /// Set the start state of a contact generation problem
     /// environment, ordered by their efficiency

script/scenarios/ground_crouch_hyq_interp.py

 from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 from hpp.gepetto import Viewer
 
 import ground_crouch_hyq_path as tp
@@ -26,45 +27,43 @@ r = tp.Viewer (ps)
 
 rootName = 'base_joint_xyz'
 
-#~ AFTER loading obstacles
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
 cType = "_3_DOF"
+# string identifying the limb
 rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
 rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
 rfoot = 'rf_foot_joint'
-rLegOffset = [0.,0,0.]
-rLegNormal = [0,1,0]
-rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.1,cType)
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
 
 lLegId = 'lhleg'
 lLeg = 'lh_haa_joint'
 lfoot = 'lh_foot_joint'
-lLegOffset = [0,0,0]
-lLegNormal = [0,1,0]
-lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "backward", 0.05,cType)
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
 
 rarmId = 'rhleg'
 rarm = 'rh_haa_joint'
 rHand = 'rh_foot_joint'
-rArmOffset = [0.,0,-0.]
-rArmNormal = [0,1,0]
-rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "backward", 0.05,cType)
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
 
 larmId = 'lfleg'
 larm = 'lf_haa_joint'
 lHand = 'lf_foot_joint'
-lArmOffset = [0.,0,-0.]
-lArmNormal = [0,1,0]
-lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05,cType)
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
 
 q_0 = fullBody.getCurrentConfig(); 
 q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
 q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
 
-
 fullBody.setCurrentConfig (q_init)
 q_init = fullBody.generateContacts(q_init, [0,0,1])
 q_0 = fullBody.getCurrentConfig(); 
@@ -84,9 +83,9 @@ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
 i = 0;
 r (configs[i]); i=i+1; i-1
 
-q0 = configs[2]
-q0 = fullBody.generateContacts(q0, [0,0,1])
-r(q0)
+#~ q0 = configs[2]
+#~ q0 = fullBody.generateContacts(q0, [0,0,1])
+#~ r(q0)
 c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
-r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+#~ r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
 

script/scenarios/ground_crouch_hyq_path.py

@@ -30,9 +30,7 @@ r = Viewer (ps)
 
 q_init = rbprmBuilder.getCurrentConfig ();
 q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
 q_goal = q_init [::]
-
 q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
 
 ps.addPathOptimizer("RandomShortcut")
@@ -44,6 +42,8 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
 r.loadObstacleModel (packageName, "groundcrouch", "planning")
 #~ ps.solve ()
 t = ps.solve ()
+
+
 if isinstance(t, list):
 	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
 f = open('log.txt', 'a')

script/scenarios/stair_bauzil_hrp2_interp.py

@@ -45,7 +45,8 @@ rHand = 'RARM_JOINT5'
 rArmOffset = [0,0,-0.1]
 rArmNormal = [0,0,1]
 rArmx = 0.024; rArmy = 0.024
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05)
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
 
 
 #~ AFTER loading obstacles

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -101,11 +101,15 @@ class FullBody (object):
     #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
     # \param heuristicName: name of the selected heuristic for configuration evaluation
     # \param loadValues: whether values computed, other than the static ones, should be loaded in memory
-    def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True):		
+    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
+    def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):		
 		boolVal = 0.
+		boolValEff = 0.
 		if(loadValues):
 			boolVal = 1.
-		self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal)		
+		if(disableEffectorCollision):
+			boolValEff = 1.
+		self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff)		
 
 	## Add a limb to the model
 	#
@@ -124,8 +128,12 @@ class FullBody (object):
     # of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
     # This can be problematic in terms of performance. The default value is 3 cm.
     # \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
-    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF"):
-		self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType)
+    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
+    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False):
+		boolValEff = 0.
+		if(disableEffectorCollision):
+			boolValEff = 1.
+		self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff)
 
 	## Returns the configuration of a limb described by a sample
 	#

src/rbprmbuilder.impl.cc

@@ -451,7 +451,7 @@ namespace hpp {
     }
 
     void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
-                               unsigned short samples, const char* heuristicName, double resolution, const char *contactType) throw (hpp::Error)
+                               unsigned short samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)
             throw Error ("No full body robot was loaded");
@@ -468,7 +468,8 @@ namespace hpp {
             {
                 cType = hpp::rbprm::_3_DOF;
             }
-            fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y, problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType);
+            fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y,
+                               problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5);
         }
         catch(std::runtime_error& e)
         {
@@ -477,14 +478,15 @@ namespace hpp {
     }
 
 
-    void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error)
+    void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)
             throw Error ("No full body robot was loaded");
         try
         {
             std::string fileName(databasePath);
-            fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5);
+            fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5,
+                               disableEffectorCollision > 0.5);
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -118,8 +118,10 @@ namespace hpp {
                                                    double octreeNodeId) throw (hpp::Error);
 
         virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
-                             unsigned short samples, const char *heuristicName, double resolution, const char *contactType) throw (hpp::Error);
-        virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error);
+                             unsigned short samples, const char *heuristicName, double resolution, const char *contactType,
+                             double disableEffectorCollision) throw (hpp::Error);
+        virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
+                                     double disableEffectorCollision) throw (hpp::Error);
 
         virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);