Commit eda600cd authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added script for comparing heuristics

parent f1d7ee7e
...@@ -265,7 +265,8 @@ module hpp ...@@ -265,7 +265,8 @@ module hpp
/// \param analysis name of the analysis existing if analysis ="all", /// \param analysis name of the analysis existing if analysis ="all",
/// all tests are run. /// all tests are run.
/// \param isstatic 1 is becomes new static value of database, 0 otherwise /// \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 }; // interface Robot
}; // module rbprm }; // module rbprm
......
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()
...@@ -264,7 +264,7 @@ class FullBody (object): ...@@ -264,7 +264,7 @@ class FullBody (object):
isStatic = 0. isStatic = 0.
if(isstatic): if(isstatic):
isStatic = 1. 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 ## Create octree nodes representation for a given limb
# #
......
...@@ -6,6 +6,8 @@ from matplotlib import cm ...@@ -6,6 +6,8 @@ from matplotlib import cm
cma = cm.autumn cma = cm.autumn
#~ plt.ion()
## Display a 3d plot of the values computed for a limb database ## 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 # where all samples take the maximum value of the octree they belong to
# \param robot FullBody object # \param robot FullBody object
...@@ -51,10 +53,10 @@ def plotcube(plt, ax, c, pos): ...@@ -51,10 +53,10 @@ def plotcube(plt, ax, c, pos):
Y, Z = np.meshgrid(y1, z1) Y, Z = np.meshgrid(y1, z1)
ax.plot_surface(x1,Y,Z, color = cma(c)) ax.plot_surface(x1,Y,Z, color = cma(c))
def plotOctreeValues(robot, valueName, limb): def getOctreeValues(robot, valueName, limb):
fig = plt.figure() res = {}
ax = fig.add_subplot(111, projection='3d') res ['boxes'] = []
#first iterate over octree. res ['values'] = []
octreeIds = robot.getOctreeNodeIds(limb) octreeIds = robot.getOctreeNodeIds(limb)
for ocId in octreeIds: for ocId in octreeIds:
sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId) sampleIds = robot.getSamplesIdsInOctreeNode(limb, ocId)
...@@ -64,15 +66,46 @@ def plotOctreeValues(robot, valueName, limb): ...@@ -64,15 +66,46 @@ def plotOctreeValues(robot, valueName, limb):
g = robot.getSampleValue(limb, valueName, i) g = robot.getSampleValue(limb, valueName, i)
max_val = max(max_val, g) max_val = max(max_val, g)
box = robot.getOctreeBox(limb, (int)(ocId)) box = robot.getOctreeBox(limb, (int)(ocId))
plotcube(plt,ax,max_val, box) res['boxes'].append(box)
#~ if box[2] < -0.15 and box[2] > -0.25: res['values'].append(max_val)
#~ plotcube(plt,ax,max_val, box) 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_xlabel('X Label')
ax.set_ylabel('Y Label') ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label') ax.set_zlabel('Z Label')
plt.title(valueName) plt.title(valueName)
plt.show() 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 ## Display a 3d plot of the values computed for a limb database
# #
# \param robot FullBody object # \param robot FullBody object
......
...@@ -873,10 +873,11 @@ namespace hpp { ...@@ -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 try
{ {
rbprm::sampling::ValueBound bounds;
if(!fullBodyLoaded_) if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded"); throw Error ("No full body robot was loaded");
T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname)); T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
...@@ -893,6 +894,7 @@ namespace hpp { ...@@ -893,6 +894,7 @@ namespace hpp {
{ {
sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_); sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5); sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
bounds = sampleDB.valueBounds_[analysisit->first];
} }
} }
else else
...@@ -905,7 +907,13 @@ namespace hpp { ...@@ -905,7 +907,13 @@ namespace hpp {
} }
sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_); sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5); 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) catch(std::runtime_error& e)
{ {
......
...@@ -132,7 +132,7 @@ namespace hpp { ...@@ -132,7 +132,7 @@ namespace hpp {
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error); 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 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 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: public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver); void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment