added script for comparing heuristics

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -265,7 +265,8 @@ module hpp
     /// \param analysis name of the analysis existing if analysis ="all",
     /// all tests are run.
     /// \param isstatic 1 is becomes new static value of database, 0 otherwise
-    void runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
+    /// \return min and max values obtained
+    floatSeq runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
 
   }; // interface Robot
   }; // module rbprm

script/scenarios/olivier_urdf.py

+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+import hpp.corbaserver.rbprm.tools.plot_analytics  as plot_ana
+
+def loadRobot(packageName,meshPackageName,rootJointType,urdfName,urdfSuffix,srdfSuffix, limbId, limbRoot, limbEffector):
+	fullBody = FullBody ()
+	fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+	fullBody.setJointBounds ("base_joint_xyz", [0,0,0,0,0,0])
+
+	limbOffset = [0,0,0] #inutile ici
+	limbNormal = [0,1,0] #inutile ici
+	limbx = 0.09; limby = 0.05 #inutile ici
+	fullBody.addLimb(limbId,limbRoot,'',limbOffset,limbNormal, limbx, limby, 10000, "manipulability", 0.1)
+	return fullBody
+
+def runall(lid, valueNames):	
+	res = {} #dictionnaire des values min / max pour chaque critere
+	#~ fullBody.runLimbSampleAnalysis(lid, "jointLimitsDistance", False)
+	for valueName in valueNames:
+		test = fullBody.runLimbSampleAnalysis(lid, valueName, False)
+		res [valueName] = test
+	return res
+
+def retrieveOctreeValues(fullBody, valueNames,limbId):
+	res = {}	
+	for valueName in valueNames:
+		res [valueName] = plot_ana.getOctreeValues(fullBody, valueName, limbId)
+	return res
+	
+def rescaleOctreeValue(valueName, robotData):
+	r1_min = robotData[0]["valueBounds"][valueName][0]
+	r2_min = robotData[1]["valueBounds"][valueName][0]
+	r1_max = robotData[0]["valueBounds"][valueName][1]
+	r2_max = robotData[1]["valueBounds"][valueName][1]
+	r_min = min(r1_min, r2_min)
+	r_max = min(r1_max, r2_max)
+	
+	for i in range(0,len(robotData[0]["octreeValues"][valueName]['values'])):
+		val = robotData[0]["octreeValues"][valueName]['values'][i]
+		robotData[0]["octreeValues"][valueName]['values'][i] = ((val * r1_max + r1_min) - r_min) / r_max
+		
+	for i in range(0,len(robotData[1]["octreeValues"][valueName]['values'])):
+		val = robotData[1]["octreeValues"][valueName]['values'][i]
+		robotData[1]["octreeValues"][valueName]['values'][i] = ((val * r2_max + r2_min) - r_min) / r_max
+	
+def rescaleOctreeValues(valueNames, robotData):
+	for valueName in valueNames:
+		rescaleOctreeValue(valueName, robotData)
+
+#define values to analyze #EDIT
+valueNames = [
+"isotropy", 	#whole jacobian
+"isotropyRot", 	#rotation jacobian
+"isotropyTr", 	#translation jacobian
+#~ "minimumSingularValue",
+#~ "minimumSingularValueRot",
+#~ "minimumSingularValueTr",
+#~ "maximumSingularValue",
+#~ "maximumSingularValueRot",
+#~ "maximumSingularValueTr",
+#~ "manipulabilityRot",
+#~ "manipulabilityTr",
+#~ "manipulability"
+]
+
+robotData = [{},{}]
+
+#first load first robot data
+packageName = "hrp2_14_description" #EDIT
+meshPackageName = "hrp2_14_description" #EDIT
+rootJointType = "freeflyer" #EDIT
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14" #EDIT
+urdfSuffix = "_reduced" #EDIT
+srdfSuffix = "" #EDIT
+
+limbId = '0rLeg' #nom que tu souhaites, peu importe #EDIT
+limbRoot = 'RLEG_JOINT0' #joint racine de la chaine a analyser #EDIT
+limbEffector = '' # joint qui correspond a l'effecteur, laisse vide si dernier joint #EDIT
+fullBody = loadRobot(packageName,meshPackageName,rootJointType,urdfName,urdfSuffix,srdfSuffix, limbId, limbRoot, limbEffector)
+
+# run analysis
+limbValueBounds =  runall(limbId, valueNames)
+
+# compute normalized octree cube values
+robotData[0]["octreeValues"] = retrieveOctreeValues(fullBody, valueNames, limbId)
+robotData[0]["valueBounds"] = limbValueBounds
+robotData[0]["name"] = urdfName + limbId
+
+#now to the second robot
+
+packageName = "hrp2_14_description" #EDIT
+meshPackageName = "hrp2_14_description" #EDIT
+rootJointType = "freeflyer" #EDIT
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14" #EDIT
+urdfSuffix = "_reduced" #EDIT
+srdfSuffix = "" #EDIT
+
+limbId = '3Rarm' #EDIT
+limbRoot = 'RARM_JOINT0' #EDIT
+limbEffector = 'RARM_JOINT5' #EDIT
+fullBody = loadRobot(packageName,meshPackageName,rootJointType,urdfName,urdfSuffix,srdfSuffix, limbId, limbRoot, limbEffector)
+
+# run analysis
+limbValueBounds =  runall(limbId, valueNames)
+# compute normalized octree cube values
+robotData[1]["octreeValues"] = retrieveOctreeValues(fullBody, valueNames, limbId)
+robotData[1]["valueBounds"] = limbValueBounds
+robotData[1]["name"] = urdfName + limbId
+
+rescaleOctreeValues(valueNames, robotData)
+for valueName in valueNames:
+	plot_ana.compareOctreeValues(robotData[0]["name"], robotData[1]["name"], robotData[0]["octreeValues"][valueName], robotData[1]["octreeValues"][valueName], valueName)
+	
+import matplotlib.pyplot as plt
+plt.show()

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -264,7 +264,7 @@ class FullBody (object):
 		isStatic = 0.
 		if(isstatic):
 			isStatic = 1.
-		self.client.rbprm.rbprm.runLimbSampleAnalysis(limbname, analysis,isStatic)
+		return self.client.rbprm.rbprm.runLimbSampleAnalysis(limbname, analysis,isStatic)
 		
 	## Create octree nodes representation for a given limb
 	#

src/hpp/corbaserver/rbprm/tools/plot_analytics.py

@@ -6,6 +6,8 @@ from matplotlib import cm
 
 cma = cm.autumn
 
+#~ plt.ion()
+
 ## Display a 3d plot of the values computed for a limb database
 # where all samples take the maximum value of the octree they belong to
 # \param robot FullBody object
@@ -51,10 +53,10 @@ def plotcube(plt, ax, c, pos):
 	Y, Z = np.meshgrid(y1, z1)
 	ax.plot_surface(x1,Y,Z, color = cma(c))
 
-def plotOctreeValues(robot, valueName, limb):
-	fig = plt.figure()
-	ax = fig.add_subplot(111, projection='3d')
-	#first iterate over octree.
+def getOctreeValues(robot, valueName, limb):
+	res = {}
+	res ['boxes']  = []
+	res ['values'] = []
 	octreeIds = robot.getOctreeNodeIds(limb)
 	for ocId in octreeIds:
 		sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
@@ -64,15 +66,46 @@ def plotOctreeValues(robot, valueName, limb):
 			g = robot.getSampleValue(limb, valueName, i)
 			max_val = max(max_val, g)
 		box = robot.getOctreeBox(limb, (int)(ocId))
-		plotcube(plt,ax,max_val, box)
-		#~ if box[2] < -0.15 and box[2] > -0.25:
-			#~ plotcube(plt,ax,max_val, box)
+		res['boxes'].append(box)
+		res['values'].append(max_val)
+	return res
+		
+
+def plotOctreeValues(robot, valueName, limb):
+	fig = plt.figure()
+	ax = fig.add_subplot(111, projection='3d')
+	boxesValues = getOctreeValues(robot, valueName, limb)
+	boxes = res['boxes']
+	values = res['values']
+	for i in range(0,len(boxes)):
+		plotcube(plt,ax,values[i], boxes[i])
 	ax.set_xlabel('X Label')
 	ax.set_ylabel('Y Label')
 	ax.set_zlabel('Z Label')
 	plt.title(valueName)
 	plt.show()
 
+def plotOctreeValuesCompare(ax, boxesValues):
+	boxes = boxesValues['boxes']
+	values = boxesValues['values']
+	for i in range(0,len(boxes)):
+		plotcube(plt,ax,values[i], boxes[i])
+	ax.set_xlabel('X Label')
+	ax.set_ylabel('Y Label')
+	ax.set_zlabel('Z Label')
+
+def compareOctreeValues(robotName1, robotName2, boxesValues1, boxesValues2, valueName):
+	fig = plt.figure()
+	fig.suptitle(valueName)
+	ax = fig.add_subplot(121, projection='3d')
+	ax.set_title(robotName1)
+	plotOctreeValuesCompare(ax, boxesValues1)
+	bx = fig.add_subplot(122, projection='3d')
+	bx.set_title(robotName2)
+	plotOctreeValuesCompare(bx, boxesValues2)
+	#~ plt.title(valueName)
+	plt.draw()
+
 ## Display a 3d plot of the values computed for a limb database
 #
 # \param robot FullBody object

src/rbprmbuilder.impl.cc

@@ -873,10 +873,11 @@ namespace hpp {
         }
     }
 
-    void RbprmBuilder::runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error)
+    hpp::floatSeq* RbprmBuilder::runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error)
     {
         try
         {
+        rbprm::sampling::ValueBound bounds;
         if(!fullBodyLoaded_)
             throw Error ("No full body robot was loaded");
         T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
@@ -893,6 +894,7 @@ namespace hpp {
             {
                 sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
                 sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+                bounds = sampleDB.valueBounds_[analysisit->first];
             }
         }
         else
@@ -905,7 +907,13 @@ namespace hpp {
             }
             sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
             sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
-            }
+            bounds = sampleDB.valueBounds_[analysisit->first];
+        }
+        hpp::floatSeq* dofArray = new hpp::floatSeq();
+        dofArray->length(2);
+        (*dofArray)[0] = bounds.first;
+        (*dofArray)[1] = bounds.second;
+        return dofArray;
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -132,7 +132,7 @@ namespace hpp {
         virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
         virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
         virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
-        virtual void runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
+        virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
 
         public:
         void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);