diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 0338729c7d0eddc4171570d02d008871f74818bc..7c2af6b534434278dcdeb70b85b8175f0397103e 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/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 diff --git a/script/scenarios/olivier_urdf.py b/script/scenarios/olivier_urdf.py new file mode 100644 index 0000000000000000000000000000000000000000..34b880b318ad39bcbf5b45a5f09d4982102e0fb3 --- /dev/null +++ b/script/scenarios/olivier_urdf.py @@ -0,0 +1,118 @@ +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() diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index a5ada65ab5c65346d959770e4b2cedb05186ca5d..f6091f2f81e04ec5ba33563e58f9cf79cc781223 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/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 # diff --git a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py b/src/hpp/corbaserver/rbprm/tools/plot_analytics.py index a2783bc87a06aaa8f602ae7e6f8503d13b4a9b2a..b78e06628d8a0d55fad53dee7041abd790ef7d8f 100644 --- a/src/hpp/corbaserver/rbprm/tools/plot_analytics.py +++ b/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 diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index 49bf174faea81ac1c41d0120cd7c6f586006816b..2bb3a01f0c6c558ba5cd7efcafa5d29ee0361fbc 100755 --- a/src/rbprmbuilder.impl.cc +++ b/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) { diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 3a7ead14a8521342cbff69f901dea653de775aaa..9535fd002fae9d3681ab073cf5ac7bea56b23fc1 100755 --- a/src/rbprmbuilder.impl.hh +++ b/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);